Multi-stage maximum likelihood target estimator

ABSTRACT

A multi-stage maximum likelihood target estimator for use with radar and sonar systems is provided. The estimator is a software implemented algorithm having four computational stages. The first stage provides angle smoothing for data endpoints thereby reducing angle errors associated with tie-down times. The second stage performs a coarse grid search to obtain the initial approximate target state to be used as a starting point for stages  3  and  4 . The third stage is an endpoint Gauss-Newton type maximum likelihood target estimate which determines target range along two time lines. The final refinement of the target state is obtained by the fourth stage which is a Cartesian coordinate maximum likelihood target estimate. The four-stage processing allows the use of target historic data while reducing processing time and computation power requirement.

STATEMENT OF GOVERNMENT INTEREST

The invention described herein may be manufactured and used by or forthe Government of the United States of America for Governmental purposeswithout the payment of any royalties thereon or therefor.

BACKGROUND OF THE INVENTION

(1) Field of the Invention

The present invention relates generally to the field of radar and sonarsystems. In particular, the invention employs an algorithm in a processfor enhanced target detection and tracking.

(2) Description of the Prior Art

State of the art combat systems rely heavily on target motion analysis(TMA) subcomponents. A target motion analysis subcomponent estimates thecurrent position and velocity components of a contact. Estimates fromtarget motion analysis are important to combat personnel because theestimate allows the personnel to predict the location of the hostilecontact some time in the future. Precise determination of the futureposition of the contact is required for accurate targeting of weaponssystems as well as for defensive maneuvering and evasion of the contactby friendly units.

In both radar and sonar detection systems, an antenna array receives areflected signal. Preliminary processing then occurs and the locationsof contacts are generated. An example of this type of processing isdisclosed in Chang et al., Active Sonar Range-Beam Partitioner, U.S.Pat. No. 5,737,249 (Filed 7 Apr. 1998).

The next stage in processing is to determine range and bearing estimatesfor each target. Prior attempts have led to two distinct approaches forthese determinations. The first approach, (sequential algorithms) usesan averaged measurement to reflect historic information and combinesthis average in a weighted manner with the most current measurement.This approach yields minimal computational needs due to the small sizeof the input dataset. Sequential algorithms also can respond quickly totargets that have rapidly varying direction of movement. However, thecondensation of all historic measurements into a single set of inputnumbers results in a great loss in the granularity of the data.Sequential algorithms have not been able to utilize the completehistoric dataset to dynamically recompute the output range and bearingas a cache set of input values is received.

Batch processing algorithms have developed to meet this precise need.However, batch processing algorithms have also been plagued with aplethora of problems. First, computational requirements haveconsistently been exceptionally high. As a result, algorithm designershave been limited in the amount of processing steps which could beperformed while still providing real time output. In some circumstances,computational needs have been so high as to require limiting the numberof individual historic input measurements which are processed. As such,all viable prior attempts have used a single stage algorithm forprocessing.

The first type of algorithm often used is grid searching. The gridsearch technique divides the target space into a number of cells.Contact range and bearing are computed by detecting movement betweencells. In order for this technique to be successful, the resolution ofthe target grid must be very fine. This fine resolution has resulted inextreme computational power requirements.

The second type of algorithm is a stand-alone endpoint coordinatemaximum likelihood estimation (MLE). In maximum likelihood estimation,an iterative least-squares technique is used to determine contact rangeand bearing. However, this approach has been subject toover-sensitivity, especially in cases where iterations on the quadraticsolution lead to a divergence rather than a convergence.

SUMMARY OF THE INVENTION

Accordingly, it is an object of the present invention to provide asoftware algorithm which provides range and bearing estimates for targetacquisition systems.

It is a further object of the present invention to minimizecomputational requirements while processing substantial historic data.

It is a still further object of the present invention to maintain a highgranularity or resolution in the target field.

It is a still further object of the present invention to preventdivergence of the least-squares solution and of the target falses whichresult from such divergence.

In accordance with these and other objects, the invention is a processusing a multi-stage algorithm for estimating the current position andvelocity components of contacts. The algorithm comprises four majorstages. In the first stage, pre-processing aimed at elimination of angleerrors associated with the time measurements is developed for use inlater stages. In the second stage, a coarse grid search, in endpointcoordinates, is performed to yield a refined range estimate at each ofthe time measurements. In the third stage, an endpoint Gauss-Newton typemaximum likelihood estimation (MLE) solution is performed to yield anaccurate range estimate. Finally, in the fourth stage, the computedrange and bearing values are refined more precisely through a Cartesiancoordinate MLE.

The four-stage process or method provides the advantage of allowing eachstage of the algorithm to work with well-defined input data.Additionally, this method allows the overall algorithm to performcomputationally heavy operations over a smaller data space.

Also, the initial stage of the operation is held to a coarse estimationrequiring little processing power. In this way, the present invention isable to handle large amounts of historic target information withoutsacrificing resolution in the target space. Furthermore, the procedureof using preprocessing and early estimation stages before the leastsquares operations in the MLE stages, steps the algorithm from selectingiteration points at local minimums rather than true minimums. Thisprocedure prevents divergence in the solution and prevents the resultingfalse radar and sonar targets.

BRIEF DESCRIPTION OF THE DRAWINGS

The foregoing objects and other advantages of the present invention willbe more fully understood from the following detailed description andreference to the appended drawings wherein:

FIG. 1 is a block diagram depicting the multi-stage maximum likelihoodestimation (MLE) method;

FIG. 2 is a geographic plot of MLE endpoints; and

FIG. 3 is a three-dimensional representation of the sonar beam andbottom reflected beam of a towed array sensor.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

The multistage maximum likelihood estimator (MLE) processes sonar dataand computes a target solution (range, bearing, course, speed) and alocalization ellipse by processing data in several stages. The algorithmprocesses azimuthal bearing measurements, direct path or bottom-bounceconical angle measurements, horizontal range, direct path or bottombounce frequency measurements from multiple trackers and sonar arrays.Frequency data from a maximum of 2 trackers may be processed. Thealgorithm constraints include a non-maneuvering target at a known depth,a flat ocean bottom, and an isovelocity environment (straight-line soundpropagation). The propagation path is constrained to be either directpath or bottom bounce-on ray reversal and the measurement noise isassumed to be uncorrelated. When measurement data has been partitionedinto segments, propagation path hypothesis testing is performed.

Referring now to FIG. 1, the overall process 10 is depicted showing thefour major stages of the present invention, a endpoint angle smoothingstage 12, a coarse endpoint coordinate grid search stage 22, an endpointGauss-Newton type MLE 32, and a fourth stage, the Cartesian coordinateMLE stage 42.

In the first stage 12, the algorithm calculates angle smoothing on theangle measurements at the endpoints of the data window in order toreduce angle errors associated with the tie down times (depicted in FIG.2 as time line 1 (t₁) and time line 2 (t₂)) used by the endpoint coarsegrid search and endpoint maximum likelihood estimator.

In the second stage 22 of FIG. 1, a coarse grid search in endpointcoordinates is performed to obtain a reasonable initial stage estimateof target range at the two times lines. Referring again to FIG. 2, thetarget position at time line 1 (t₁) and time line 2 (t₂) is constrainedto lie on either the azimuthal bearing lines or conical angle hyperbolasfor bottom bounce propagation or conical angle hyperbolic asymptotes fordirect path propagation, thereby producing the constrained track of atarget 26. The actual track 28 is depicted showing the convergence ofthe solution.

These target restraints may be better visualized by reference to FIG. 3.In FIG. 3, ownship 62 is submerged at a submarine depth plane 68 with arepresentation of the sonar-emitted, cone-shaped beam 64. Thecone-shaped beam 64 either directly impinges a target or can bereflected off the ocean bottom 72. As shown, the bottom reflection 66produces a hyperbola. As a result, the reflected beam 70 is a conicalangle hyperbola.

In the third stage 32, an endpoint Gauss-Newton type MLE estimatestarget range at the two times lines along with a target base frequencyfor a maximum of two frequency trackers. Again, the target position attime line 1 and time line 2 is constrained to lie on either theazimuthal bearing lines, conical angle hyperbolas or conical anglehyperbolic asymptotes.

In the fourth stage 42, the solution is further refined using theCartesian coordinate MLE, which also provides errors bounds on varioustarget parameters. The Cartesian coordinate MLE is also Gauss-Newtontype MLE that estimates target x, y-position and velocity using the sameassumptions made by the endpoint MLE.

Endpoint Angle Smoothing

The first stage, the endpoint angle smoothing stage receives input datafrom the target tracker, in this example, a sonar sensor, and providespreliminary data for follow-on stages. The algorithm performs anglesmoothing on the angle measurements at the endpoints of the data windowin order to reduce angle errors associated with the tie-down times(referred to as time line 1 and time line 2).

Because the coarse grid search constrains its target solution to lie onthe azimuthal bearing lines or conical angle hyperbolae (bottom bounce)or conical angle hyperbolic asymptotes (direct path) at time line 1 andtime line 2, significantly noisy measurements at either timeline mayresult in a significantly biased target solution. In order to avoidbiased solutions due to endpoint constraints, the coarse grid searchconstrains the target track to lie on the smoothed (vice measured)bearing lines or conical angle hyperbolae/asymptotes. The anglemeasurements from the tracker or trackers associated with time line 1and time line 2 are smoothed by fitting measurement data collectedwithin a specified time window of either time line 1 or time line 2 witha quadratic model using standard (normal equation) least-squares theory.Sophisticated orthogonalization techniques are simply not necessary inthis application.

Assuming a quadratic model, the angle measurements from the trackerassociated with time line 1 that are within 120 seconds of time line 1(a₁, a₂, . . . , a_(m)) can be described as

$\begin{matrix}{\begin{bmatrix}a_{1} \\a_{2} \\\vdots \\a_{m}\end{bmatrix} = {\begin{bmatrix}1 & {\Delta\; t_{1}} & \frac{\Delta_{1}^{2}}{2} \\1 & {\Delta\; t_{2}} & \frac{\Delta_{2}^{2}}{2} \\\vdots & \vdots & \vdots \\1 & {\Delta\; t_{m}} & \frac{\Delta_{m}^{2}}{2}\end{bmatrix}\begin{bmatrix}a_{0} \\a_{0}^{\prime} \\a_{0}^{''}\end{bmatrix}}} & (1)\end{matrix}$orz=Hx  (2)where Δt_(i) is the time of the ith measurement of time line 1

a_(i) is the ith angle measurement (−π≦a_(i)<+π)

a₀ is the smoothed angle at time line 1

a′₀ is the angle rate at time line 1

a″₀ is the angle acceleration at time line 1

The curve fit coefficients (x) can be computed using a standardunweighted normal equation approach asx=[H ^(T) H] ⁻¹ H ^(T) z  (3)where the matrix inverse is performed using a standard Gaussianelimination method. In order to tie down to the smoothed angle at timeline 1, the smoothed angle estimate a₀ can be substituted for themeasured angle at time line 1.

In similar fashion, a smoothed angle estimate at time line 2 can begenerated using tracker data associated with the time line 2 trackerthat is within 120 seconds of time line 2, and this smoothed angle canalso be substituted for the measured angle at time line 2.

If the root mean square (RMS) error of the curve fit at either time lineexceeds 3°, then the smoothed angle estimates shall be discarded.

Coarse Grid Search

The coarse grid search can process frequency data for up to two separatefrequency trackers. For improved clarity, only a single frequencytracker is described.

1. Where at least three frequency measurements are available for a givenfrequency tracker, frequency data from that tracker is processed and theestimated base frequency for that tracker (Fb) is set to the most recentfrequency measurement.

2. Set the minimum and maximum range at t1 with respect to the sensorassociated with time line 1 (R1 _(min), R1 _(max)) and the minimum andmaximum range at t2 with respect to the sensor associated with time line2 (R2 _(min), R2 _(max)) as follows:

-   -   a. If the measurement at time line 1 is a bearing, set the        minimum range at t1 with respect to the sensor associated with        time line 1 (R1 _(min)) to the minimum range constraint which is        defaulted to 100.    -   b. If the measurement at time line 1 is a conical angle, compute        the minimum range at t1 with respect to the sensor associated        with time line 1 (R1 _(min)). If R1 _(min) is less than the        minimum range constraint, set R1 _(min) to the minimum range        constraint which is defaulted to 100. The minimum range with        respect to the sensor is computed as follows:        -   i. Compute the plane depth (Rz) associated with a            measurement as follows:            -   1.) If the propagation path is direct (zero ray                reversals), then the image plane depth is computed as                follows:                Rz=Zt−Zs  (4)    -   where Zt is the assumed target depth and        -   Zs is the sensor depth            -   2.) If the propagation path is bottom bounce (one ray                reversal), then the image plane depth is computed as                follows:                Rz=2Zb−Zs−Zt  (5)    -   where Zb is the bottom depth.        -   ii. Compute the maximum D/E angle with respect to the sensor            (θ max). If the measured conical angle (βm) is between 0 and            π/2 inclusive,            θ_(max)=β_(n) −C _(s)  (6)        -    where C_(s) is the sensor cant angle. If the measured            conical angle is less than π,            θ_(max)=π−β_(m) +C _(s)  (7)        -   iii. The minimum range with respect to the sensor (R_(min))            can then be computed as

$\begin{matrix}{R_{\min} = \frac{R_{z}}{\tan\;\theta_{\max}}} & (8)\end{matrix}$

-   -   -    where R_(z) is the image plane depth.

    -   c. Set the maximum range t1 with respect to the sensor        associated with time line 1 (R1 _(max)) to the maximum range        constraint which is defaulted to 200000.

    -   d. If the measurement at time line 2 is a bearing, set the        minimum range at t2 with respect to the sensor associated with        time line 2 (R2 _(min)) to the minimum range constraint which is        defaulted to 100.

    -   e. If the measurement at time line 2 is a conical angle, compute        the minimum range at t2 with respect to the sensor associated        with time line 2 (R2 _(min)). If R2 _(min) is less than the        minimum range constraint, set R2 _(min) to the minimum        constraint which is defaulted to 100. The minimum range with        respect to the sensor is computed as for equations (4) thru (8).

    -   f. Set the maximum range at t2 with respect to the sensor        associated with time line 2 (R2 _(max)) to the maximum range        constraint which is defaulted to 200000.        3. Compute three values of range at t1 with respect to the        sensor associated with time line 1 (R1 _(j), j=1, . . . , 3) and        three values of range at t2 with respect to the sensor        associated with time line 2 (R2 _(k), k=1, . . . , 3) as        follows:        R1_(j) =R1_(min+)5000j  (9)        R2_(k) =R2_(min+)5000k  (10)        If R1 _(j)>R1 _(max), set R1 _(j) to R1 _(max). If R2 _(k)>R2        _(max), set R2 _(k) to R2 _(max).        4. If frequency data is being processed from a particular        tracker, then five base frequency estimates (Fb_(l), l=1, . . .        , 5) are computed as follows:        Fb _(l) =Fr _(avg)+0.005(l−1)  (11)        where Fr_(avg) is averaged measured frequency measurement        between t1 and t2.        5. For each combination of R1 _(j),R2 _(k),Fb₁ compute the        Endpoint coordinate performance index (PI_(jkl)) as follows:

    -   a. Compute Endpoint Parameters as follows:        -   i. If the measurement at time line 1 is a bearing, set true            bearing at t1 with respect to the sensor associated with            time line 1 (B1) to the bearing estimate at time line 1.        -   ii. If the measurement at time line 1 is a conical angle,            -   1.) Compute the target image depth at t1 with respect to                the sensor associated with time line 1 (Rz1) as                described in for equations (4) and (5).            -   2.) Compute the maximum depression/elevation (D/E) angle                at t1 with respect to the sensor associated with time                line 1 (θ1 _(max)) as described for equations (6) thru                (8).            -   3.) Compute the slant range at t1 with respect to the                sensor associated with time line 1 (Rs1):                Rs1=√{square root over (R1²+Rz1²)}  (12)            -   4.) Compute the D/E angle at t1 with respect to the                sensor associated with time line 1 (θ1):

$\begin{matrix}{{\theta\; 1} = {\sin^{- 1}\left( \frac{Rz1}{Rs1} \right)}} & (13)\end{matrix}$

-   -   -   -   5.) If θ1>θ1 _(max), the D/E angle is invalid and                processing shall terminate.            -   6.) Compute the cosine of relative bearing at t1 with                respect to the sensor associated with time line 1 (cBr1)                as follows:

$\begin{matrix}{{c\; B\; r\; 1} = \frac{{\cos\;\beta\; 1} + {\sin\; C\; s\; 1\;\sin\;\theta\; 1}}{\cos\; C\; s\; 1\;\cos\;\theta\; 1}} & (14)\end{matrix}$

-   -   -   where Cs1 is the cant angle at t1 of the sensor associated            with time line 1            -   β1 is the conical angle estimate at time line 1.            -   7.) Insure that −0.99999<cBr1<0.99999.            -   8.) Compute the relative bearing at t1 with respect to                the sensor associated with time line 1 (Br1) as follows:                Br1=cos⁻¹ cBr1  (15)            -   9.) If the port/starboard assumption for time line 1                indicates port, set Br1=2π−Br1.            -   10.) Compute the true bearing at t1 with respect to the                sensor associated with time line 1 (B1) as follows:                B1=Br1+Hs1  (16)        -   where Hs1 is the heading at t_(i) of the sensor associated            with time line 1.        -   iii. If the measurement at time line 2 is a bearing, set            true bearing at t2 with respect to the sensor associated            with time line 2 (B2) to the bearing estimate at time line            2.        -   iv. If the measurement at time line 2 is a conical angle,            -   1.) Compute the target image depth at t2 with respect to                the sensor associated with time line 2 (Rz2) as                described for equations (4) and (5).            -   2.) Compute the maximum D/E angle at t2 with respect to                the sensor associated with time line 2 (θ2 _(max)) as                described for equations (6) thru (8).            -   3.) Compute the slant range at t2 with respect to the                sensor associated with time line 2 (Rs2):                Rs2=√{square root over (R2²+Rz2²)}  (17)            -   4.) Compute the D/E angle at t2 with respect to the                sensor associated with time line 2 (θ2):

$\begin{matrix}{{\theta\; 2} = {\sin^{- 1}\left( \frac{Rz2}{Rz2} \right)}} & (18)\end{matrix}$

-   -   -   -   5.) If θ2≧θ2 _(max), the D/E angle is invalid and                processing shall terminate.            -   6.) Compute the cosine of relative bearing at t2 with                respect to the sensor associated with time line 2 (cBr2)                as follows:

$\begin{matrix}{{c\; B\; r\; 2} = \frac{{\cos\;\beta\; 2} + {\sin\; C\; s\; 2\;\sin\;\theta\; 2}}{\cos\; C\; s\; 2\;\cos\;\theta\; 2}} & (19)\end{matrix}$

-   -   -   -    where Cs2 is the cant angle at t2 of the sensor                associated with time line 2            -   β2 is the conical angle estimate at time line 2.            -   7.) Insure that −0.99999<cBr2<0.99999.            -   8.) Compute the relative bearing at t2 with respect to                the sensor associated with time line 2 (Br2) as follows:                Br2=cos⁻¹ cBr2  (20)            -   9.) If the port/starboard assumption for time line 2                indicates port set Br2=2π−Br2.            -   10.) Compute the true bearing at t2 with respect to the                sensor associated with time line 2 (B2) as follows:                B2=Br2+Hs2  (21)            -    where Hs2 is the heading at t2 of the sensor associated                with time line 2.

    -   b. For each measurement in the batch:        -   i. Compute the x-component of range t_(i) with respect to            the sensor associated with the ith measurement (Rx_(i)) and            the y-component of range at t_(i) with respect to the sensor            associated with the ith measurement (Ry_(i)):

$\begin{matrix}{{T1}_{i} = \frac{t_{i} - {t1}}{{t2} - {t1}}} & (22)\end{matrix}$T2_(i)=1−T1₁  (23)Rx _(i) =T2_(i) R1_(j) sin B1+T1_(i) R2_(k) sin B2+T1_(i)(Xs2−Xs1)−(Xs_(i) −Xs1)  (24)Ry _(i) =T2_(i) R1_(j) cos B1+T1_(i) R2_(k) cos B2+T1_(i)(Ys2−Ys1)−(Ys_(i) −Ys1)  (25)

-   -   where Xs_(i) is the x-coordinate of the position at t_(i) of the        sensor associated with the ith measurement Ys_(i) is the        y-coordinate of the position at t_(i) of the sensor associate        with the ith measurement t_(i) the time of the ith measurement        -   ii. If the ith measurement is a bearing, the following shall            be performed:            -   1.) Compute the true bearing at t_(i) with respect to                the sensor associated with the ith measurement (B_(i)):

$\begin{matrix}{B_{i} = {\tan^{- 1}\left( \frac{{Rx}_{i}}{{Ry}_{i}} \right)}} & (26)\end{matrix}$

-   -   -   -   2.) Compute the bearing residual (RESB_(i)) such that                −π≦RESB_(i)≦π:                RESB _(i) =Bm _(i) −B _(i)  (27)            -    where Bm_(i) is the measured bearing at t_(i)            -   3.) Compute the normalized bearing residual ( RESB_(i)                ):

$\begin{matrix}{\overset{\_}{{RESB}_{i}} = \frac{{RESB}_{i}}{\sigma\; B_{i}}} & (28)\end{matrix}$

-   -   -   where σB_(i) is the standard deviation of the measured            bearing at t_(i)        -   iii. If the ith measurement is a conical angle, the            following shall be performed:            -   1.) Compute the target image depth at t_(i) with respect                to the sensor associated with ith measurement (Rz_(i))                as described for equations (4) and (5).            -   2.) Compute the maximum D/E angle at t_(i) with respect                to the sensor associated with the ith measurement                (θ_(maxi)) as described for equations (6) thru (8).            -   3.) Compute the slant range at t_(i) with respect to the                sensor associated with the ith measurement (Rs_(i)):

$\begin{matrix}{{Rs}_{i} = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2} + {Rz}_{i}^{2}}} & (29)\end{matrix}$

-   -   -   -   4.) Compute the D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)):

$\begin{matrix}{\theta_{i} = {\sin^{- 1}\left( \frac{{Rz}_{i}}{{Rs}_{i}} \right)}} & (30)\end{matrix}$

-   -   -   -   5.) If θ_(i)<θ_(maxi), the D/E angle is valid and the                following shall be performed:                -   a Compute the x-component of range at t_(i) with                    respect to the sensor associated with the ith                    measurement (xta_(i)), the y-component of range at                    t_(i) with respect to the sensor associated with the                    ith measurement (yta_(i)) and the z-component of                    range at t_(i) with respect to the sensor associated                    with the ith measurement rotated to the axis of the                    array (zta_(i))                    xta _(i) =Rx _(i) cos Hs _(i) −Ry _(i) sin Hs                    _(i)  (31)                    yta _(i)=(Rx _(i) sin Hs _(i) +Ry _(i) cos Hs                    _(i))cos Cs _(i) −Rz _(i) sin Cs _(i)  (32)                    zta _(i)=(Rx _(i) sin Hs _(i) +Ry _(i) cos Hs                    _(i))sin Cs _(i) +Rz _(i) cos Cs _(i)  (33)                -    where CS_(i) is the cant angle at t_(i) of the                    sensor associated with the ith measurement                -   Hs_(i) is the heading at t_(i) of the sensor                    associated with the ith measurement                -   b Compute the conical angle at ti with respect to                    the sensor associated with the ith measurement                    (β_(i)):                -   If yta_(i)≠0

$\begin{matrix}{\beta_{i} = {\tan^{- 1}\left( \frac{\sqrt{{xta}_{i}^{2} + {zta}_{i}^{2}}}{{yta}_{i}} \right)}} & (34)\end{matrix}$

-   -   -   -   -    otherwise

$\begin{matrix}{\beta_{i} = \frac{\pi}{2}} & (35)\end{matrix}$

-   -   -   -   -   c. Compute the conical angle residual (RESβ_(i))                    such that −π≦RESβ_(i)≦π:                    RESβ _(i) =βm _(i)−β₁  (36)                -    where βm_(i) is the measured conical angle at                    t_(i).                -   d. Compute the normalized conical angle residual (                    RESβ_(i) ):

$\begin{matrix}{\overset{\_}{{RES}\;\beta_{i}} = \frac{{RES}\;\beta_{i}}{{\sigma\beta}_{i}}} & (37)\end{matrix}$

-   -   -   -   -    where σβ_(i) is the standard deviation of the                    measured conical angle at t_(i).

        -   iv. If the ith measurement is a horizontal range:            -   1.) Compute the range at t_(i) with respect to the                sensor associated with the ith measurement (R_(i)):

$\begin{matrix}{R_{i} = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2}}} & (38)\end{matrix}$

-   -   -   -   2.) Compute the range residual (RESR_(i)):                RESR _(i) =Rm _(i) −R _(i)  (39)            -    where Rm_(i) is the measured range at t_(i).            -   3.) Compute the normalized range residual ( RESR_(i) ):

$\begin{matrix}{\overset{\_}{{RESR}_{i}} = \frac{{RESR}_{i}}{\sigma\; R_{i}}} & (40)\end{matrix}$

-   -   -   -    where σR_(i) is the standard deviation of the measured                range at t_(i).

        -   v. If the ith measurement is a frequency and frequency data            are being processed:            -   1.) Compute the target image at t_(i) with respect to                the sensor associated with the ith measurement (Rz_(i))                as described for equations (4) and (5).            -   2.) Compute the maximum D/E angle at t_(i) with respect                to the sensor associated with the ith measurement                (θ_(maxi)) as described for equations (6) thru (8).            -   3.) Compute the slant range at t_(i) with respect to the                sensor associated with the ith measurement (Rs_(i)):

$\begin{matrix}{{Rs}_{i} = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2} + {Rz}_{i}^{2}}} & (41)\end{matrix}$

-   -   -   -   4.) Compute the D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)):

$\begin{matrix}{\theta_{i} = {\sin^{- 1}\left( \frac{{Rz}_{i}}{{Rs}_{i}} \right)}} & (42)\end{matrix}$

-   -   -   -   5.) If θ_(i)<θ_(maxi), the D/E angle is valid and the                following shall be performed:                -   a Compute the x-component of target velocity (Vxt)                    and the y-component of target velocity (Vyt):

$\begin{matrix}{{Vxt} = \frac{{{R2}_{k}\sin\;{B2}} + {Xs2} - {{R1}_{j}\sin\;{B1}} - {Xs1}}{{t2} - {t1}}} & (43) \\{{Vyt} = \frac{{{R2}_{k}\cos\;{B2}} + {Ys2} - {{R1}_{j}\cos\;{B1}} - {Ys2}}{{t2} - {t1}}} & (44)\end{matrix}$

-   -   -   -   -   b. Compute the frequency at t_(i) with respect to                    the sensor associated with the ith measurement                    (F_(i)):

$\begin{matrix}{F_{i} = {{Fb}\frac{{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}}{{cRs}_{i} + {VxtRx}_{i} + {VytRy}_{i}}}} & (45)\end{matrix}$

-   -   -   -   -    where c is the average speed of sound.                -   c. Compute the frequency residual ( RESF_(i) ):                    RESF _(i) =Fm _(i) −F _(i)  (46)                -    where Fm_(i) is the measured frequency at t_(i).                -   d. Compute the normalized frequency residual (                    RESF_(i) ):

$\begin{matrix}{\overset{\_}{{RESF}_{i}} = \frac{{RESF}_{i}}{\sigma\; F_{i}}} & (47)\end{matrix}$

-   -   -   -   -    where σF_(i) is the standard deviation of the                    measured frequency at t_(i).

    -   c. If a range constraint is being imposed, then the following        computations shall be performed:        -   i. Compute the target range (R) as follows:

$\begin{matrix}{R = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2}}} & (48)\end{matrix}$

-   -   -   ii. Compute the range residual (RESR):            RESR=Rc−R  (49)        -    where Rc is the assumed target range.        -   iii. Compute the normalized range residual ( RESR):

$\begin{matrix}{\overset{\_}{RESR} = \frac{RESR}{\sigma\; R}} & (50)\end{matrix}$

-   -   -    where σR is the standard deviation of the assumed target            range.

    -   d. If a speed constraint is being imposed, then the following        computations shall be performed:        -   i. Compute the x-component of target velocity (Vxt) and the            y-component of target velocity (Vyt):

$\begin{matrix}{{V\; x\; t} = \frac{{{R2}_{k}\sin\;{B2}} + {X\;{s2}} - {{R1}_{j}\sin\;{B1}} - {X\;{s1}}}{{t2} - {t1}}} & (51) \\{{V\; y\; t} = \frac{{{R2}_{k}\cos\;{B2}} + {Y\;{s2}} - {{R1}_{j}\cos\;{B1}} - {Y\;{s1}}}{{t2} - {t1}}} & (52)\end{matrix}$

-   -   -   ii. Compute the target speed (V):            V=√{square root over (Vxt²+Vyt²)}  (52)        -   iii. Compute the speed residual (RESV):            RESV=Vc−V  (54)        -    where Vc is the assumed target speed.        -   iv. Compute the normalized speed residual ( RESV):

$\begin{matrix}{\overset{\_}{RESV} = \frac{RESV}{\sigma\; V}} & (55)\end{matrix}$

-   -   -    where σV is the standard deviation of the assumed target            speed.

    -   e. Compute the Endpoint coordinate performance index (PI_(jkl))        as the square root of the mean of the squared normalized        residuals, which include measurements as well as constraints.        6. Select the value of R1 _(j), R2 _(k) and Fb_(l) associated        with the smallest PI_(jkl).        Then assuming zero mean unit variance measurements, R⁻¹=I,

The non-linear, least-squares algorithm, which employs Householdertransformations, applies to both the third and fourth stages of thetarget estimator, the endpoint MLE and the Cartesian coordinate MLE. Thesequence of operations are:

Initialization x₁ = x₀ 1 = 1, NITER Gauss-Newton iterations i = l,mmeasurement loop m = # of measurements H = ∂h(x¹⁻¹)/∂x Jacobian matrix m× ns ns = # of state variables Z = z − h(x¹⁻¹) residual vector m × 1 H =[H|z] augmented Jacobian m × (ns + 1) A = QH Householder Transformation$= \begin{bmatrix}U & Y \\0 & d\end{bmatrix}$ U upper triangular ns × nsY is normalized residual ns × 1P = U⁻¹U^(−t) state covariance matrix ns × ns Δx = U⁻¹Y correctionvector ns × 1 PI = ½[z − h(x¹⁻¹)]R⁻¹[z − h(x¹⁻¹)] initial performanceindex (scalar) X₁ = x¹⁻¹ + αΔx state update α = stepsize via line searchPI′ = ½[z − h(x₁)] R⁻¹[z − h(x₁)] updated performance index ΔPI = (PI −PI′)/PI′ change in performance index if ΔPI < threshold, exit loopconvergence testEndpoint Coordinate MLE

The endpoint coordinate MLE can process frequency data for up to twoseparate frequency trackers. For improved clarity, only a singlefrequency tracker is described.

1. Initialize the following Endpoint coordinate MLE solution parametersto zero:

Roc (range with respect to own ship at current time)

Boc (bearing with respect to own ship at current time)

Ct (target course)

Vt (target speed)

Fb (target base frequency)

2. Initialize the number of Gauss-Newton iterations to zero. A maximumof twenty-five Gauss-Newton iterations shall be performed as describedin paragraphs 15a through 15r.

3. Determine the number of state variables as follows: If a least threefrequency measurements are available, then frequency data will beprocessed, target base frequency shall be estimated and the number ofstates (ns) shall be set to three. Otherwise, the number of statevariables shall be two, frequency data shall not be processed and targetbase frequency shall not be estimated.4. Initialize values for range at t1 with respect to the sensorassociated with time line 1 (R1) and range at t2 with respect to thesensor associated with time line 2 (R2) using the outputs from thecoarse grid search.R1=R1_(init)  (56)R2=R2_(init)  (57)where R1 _(init) and R2 _(init) are output by the grid search algorithm

t1 is the time line 1 time

t2 is the time line 2 time

5. If frequency is being processed, initialize the base frequency state(Fb) with the base frequency output by the coarse gird search algorithm.

6. Compute the Endpoint coordinate performance index (PI) based on theinitial states as follows:

-   -   a. First compute endpoint parameters:        -   i. If the measurement at time line 1 is a bearing, set true            bearing at t1 with respect to the sensor associated with            time line 1 (B1) to the bearing estimate at time line 1.        -   ii. If the measurement at time line 1 is a conical angle,            -   1.) Compute the target image depth at t1 with respect to                the sensor associated with time line 1 (Rz1) as                described for equations (4) and (5).            -   2.) Compute the maximum depression/elevation (D/E) angle                at t1 with respect to the sensor associated with time                line 1 (θ1 max) as described for equations (6) thru (8).            -   3.) Compute the slant range at t1 with respect to the                sensor associated with time line 1 (Rs1):                Rs1=√{square root over (R1²+Rz1²)}  (58)            -   4.) Compute the D/E angle at t1 with respect to the                sensor associated with time line 1 (θ1):

$\begin{matrix}{{\theta\; 1} = {\sin^{- 1}\left( \frac{R\;{z1}}{R\;{s1}} \right)}} & (59)\end{matrix}$

-   -   -   -   5.) If θ1>θ1 _(max), the D/E angle is invalid and                processing shall terminate.            -   6.) Compute the cosine of relative bearing at t1 with                respect to the sensor associated with time 1 (cBr1) as                follows:

$\begin{matrix}{{c\; B\;{r1}} = \frac{{\cos\;\beta\; 1} + {\sin\; C\;{s1}\;\sin\;\theta\; 1}}{\cos\; C\;{s1}\;\cos\;{\theta 1}}} & (60)\end{matrix}$

-   -   -   -    where Cs1 is the cant angle at t1 of the sensor                associated with time line 1            -   β1 is the conical angle estimate at time line 1            -   7.) Insure that −0.99999<cBr1<0.99999.            -   8.) Compute the relative bearing at t1 with respect to                the sensor associated with time line 1 (Br1) as follows:                Br1=cos⁻¹ cBr1  (61)            -   9.) If the port/starboard assumption for time line 1                indicates port, set Br1=2π−Br1.            -   10.) Compute the tear bearing at t1 with respect to the                sensor associated with time line 1 (B1) as follows:                B1=Br1+Hs1  (62)            -    where Hs1 is the heading at t1 of the sensor associated                with time line 1.

        -   iii. If the measurement at time line 2 is a bearing, set            true bearing at t2 with respect to the sensor associated            with time line 2 (B2) to the bearing estimate at time line            2.

        -   iv. If the measurement at time line 2 is a conical angle,            -   1.) Compute the target image depth at t2 with respect to                the sensor associated with time line 2 (Rz2) as                described for equations (4) and (5).            -   2.) Compute the maximum D/E angle at t2 with respect to                the sensor associated with time line 2 (θ2 _(max)) as                described for equations (6) thru (8).            -   3.) Compute the slant range at t2 with respect to the                sensor associated with time line 2 (Rs2):                Rs2=√{square root over (R2²+Rz2²)}  (63)            -   4.) Compute the D/E angle at t2 with respect to the                sensor associated with time line 2 (θ2):

$\begin{matrix}{{\theta\; 2} = {\sin^{- 1}\left( \frac{R\;{z2}}{R\;{s2}} \right)}} & (64)\end{matrix}$

-   -   -   -   5.) If θ2>θ2 _(max), the D/E angle is invalid and                processing shall terminate.            -   6.) Compute the cosine of relative bearing at t2 with                respect to the sensor associated with time line 2 (cBr2)                as follows:

$\begin{matrix}{{CBr2} = \frac{{\cos\;\beta\; 2} + {\sin\; C\;{s2}\;\sin\;\theta\; 2}}{\cos\; C\; s\mspace{11mu}\cos\;\theta\; 2}} & (65)\end{matrix}$

-   -   -   -    where Cs2 is the cant angle at t2 of the sensor                associated with time line 2 and β2 is the conical angle                estimate at time line 2            -   7.) Insure that −0.99999<cBr2<0.99999.            -   8.) Compute the relative bearing at t2 with respect to                the sensor associated with time line 2 (Br2) as follows:                Br2=cos⁻¹ cBr2  (66)            -   9.) If the port/starboard assumption for time line 1                indicates port, set Br2=2π−Br2.            -   10.) Compute the true bearing at t2 with respect to the                sensor associated with time line 2 (B2) as follows:                B2=Br2+Hs2  (67)

    -   where Hs2 is the heading at t2 of the sensor associated with        time line 2

    -   b. Second, for each measurement in the batch:        -   i. Compute the x-component of range at t_(i) with respect to            the sensor associated with the ith measurement (Rx_(i)) and            the y-component of range at t_(i) with respect to the sensor            associated with the ith measurement (Ryi):

$\begin{matrix}{{{T1}\;}_{i} = \frac{t_{i} - {t1}}{{t2} - \;{t1}}} & (68)\end{matrix}$T2_(i)=1−T1_(i)  (69)Rx _(i) =T2_(i) R1 sin B1+T1_(i) R2 sin B2+T1_(i)(Xs2−Xs1)−(Xs _(i)−Xs1)  (70)Ry _(i) =T2_(i) R1 cos B1+T1_(i) R2 cos B2+T1_(i)(Ys2−Ys1)−(Ys _(i)−Ys1)  (71)

-   -   -    where Xs_(i) is the x-coordinate of the position at t_(i)            of the sensor associated with the ith measurement        -   Ys_(i) is the y-coordinate of the position at t_(i) of the            sensor associated with the ith measurement        -   t_(i) is the time of the ith measurement        -   ii. If the ith measurement is a bearing, the following shall            be performed:            -   1.) Compute the true bearing at t_(i) with respect to                the sensor associated with the ith measurement (B_(i)):

$\begin{matrix}{B_{i} = {\tan^{- 1}\left( \frac{R\; x_{i}}{R\; y_{i}} \right)}} & (72)\end{matrix}$

-   -   -   -   2.) Compute the bearing residual (RESb_(i)) such that                −π≦RESb _(i)≦π:                RESb _(i) =Bm _(i) −B _(i)  (73)            -    where Bm_(i) is the measured bearing at t_(i)            -   3.) Compute the normalized bearing residual ( RESb_(i)                ):

$\begin{matrix}{\overset{\_}{R\; E\; S\; b_{i}} = \frac{R\; E\; S\; b_{i}}{\sigma\; b}} & (74)\end{matrix}$

-   -   -   -    where σb_(i) is the standard deviation of the measured                bearing at t_(i).

        -   iii. If the ith measurement is a conical angle, the            following shall be performed:            -   1.) Compute the target image depth at t_(i) with respect                to the sensor associated with the ith measurement                (Rz_(i)) as described for equations (4) and (5).            -   2.) Compute the maximum D/E angle at t_(i) with respect                to the sensor associated with the ith measurement                (θ_(maxi)) as described for equations (6) thru (8).            -   3.) Compute the slant range at t_(i) with respect to the                sensor associated with the ith measurement (Rs_(i)).

$\begin{matrix}{{R\; s_{i}} = \sqrt{{R\; x_{i}^{2}} + {R\; y_{i}^{2}} + {R\; z_{i}^{2}}}} & (75)\end{matrix}$

-   -   -   -   4.) Compute the D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)):

$\begin{matrix}{\theta_{i} = {\sin^{- 1}\left( \frac{R\; z_{i}}{R\; s_{i}} \right)}} & (76)\end{matrix}$

-   -   -   -   5.) If θ_(i)<θ_(maxi), the D/E angle is valid and the                following shall be performed:                -   a. Compute the x-component of range at t_(i) with                    respect to the sensor associated with the ith                    measurement (xta_(i)), the y-component of range at                    t_(i) with respect to the sensor associated with the                    ith measurement (yta_(i)) and the z-component of                    range at t_(i) with respect to the sensor associated                    with the ith measurement (zta_(i)) rotated to the                    axis of the array:                    xta _(i) =Rx _(i) cos Hs _(i) −Ry _(i) sin Hs                    _(i)  (77)                    yta _(i)=(Rx _(i) sin Hs _(i) +Ry _(i) cos Hs                    _(i))cos Cs _(i) −Rz _(i) sin Cs _(i)  (78)                    zta _(i)=(Rx _(i) sin Hs _(i) +Ry _(i) cos Hs                    _(i))sin Cs _(i) −Rz _(i) cos Cs _(i)  (79)                -    where Cs_(i) is the cant angle at t_(i) of the                    sensor associated with the ith measurement and                    Hs_(i) is the heading at t_(i) of the sensor                    associated with the ith measurement                -   b. Compute the conical angle at t_(i) with respect                    to the sensor associated with the ith measurement                    (β_(i)):                -   If yta_(i)≠0:

$\begin{matrix}{\beta_{i} = {\tan^{- 1}\left( \frac{\sqrt{{x\; t\; a_{i}^{2}} + {z\; t\; a_{i}^{2}}}}{y\; t\; a_{i}} \right)}} & (80)\end{matrix}$

-   -   -   -   -   otherwise:

$\begin{matrix}{\beta_{i} = \frac{\pi}{2}} & (81)\end{matrix}$

-   -   -   -   -   c. Compute the conical angle residual (RESβ_(i))                    such that −π≦RESβ_(i)≦π:                    RESβ _(i) =βm _(i)−β_(i)  (82)                -    where βm_(i) is the measured conical angle at t_(i)                -   d. Compute the normalized conical angle residual (                    RESβ_(i) ):

$\begin{matrix}{{{RES}\;\beta_{i}} = \frac{{RES}\;\beta_{i}}{{\sigma\beta}_{i}}} & (83)\end{matrix}$

-   -   -   -   -    where σβ_(i) is the standard deviation of the                    measured conical angle at t_(i)

        -   iv. If the ith measurement is a horizontal a range:            -   1.) Compute the range residual (RESr_(i)):                RESr _(i) =Rm _(i) −R _(i)

        -   where Rm_(i) is the measured range at t_(i).            -   2.) Compute the normalized range residual ( RESr_(i) ):

$\begin{matrix}{\overset{\_}{R\; E\; S\; r_{i}} = \frac{R\; E\; S\; r_{i}}{\sigma\; r_{i}}} & (84)\end{matrix}$

-   -   -   -    where σr_(i) is the standard deviation of the measured                range at t_(i).

        -   v. If the ith measurement is a frequency and frequency data            are being processed, then the following shall be performed:            -   1.) Compute the target image depth at t_(i) with respect                to the sensor associated with the ith measurement                (Rz_(i)).            -   2.) Compute the maximum D/E angle at t_(i) with respect                to the sensor associated with the ith measurement                (θ_(maxi)).            -   3.) Compute the slant range at t_(i) with respect to the                sensor associated with the ith measurement (Rs_(i)):

$\begin{matrix}{{R\; s_{i}} = \sqrt{{R\; x_{i}^{2}} + {R\; y_{i}^{2}} + {R\; z_{i}^{2}}}} & (85)\end{matrix}$

-   -   -   -   4.) Compute the D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)):

$\begin{matrix}{\theta_{i} = {\sin^{- 1}\left( \frac{R\; z_{i}}{R\; s_{i}} \right)}} & (86)\end{matrix}$

-   -   -   -   5.) If θ_(i)<θ_(maxi) the D/E angle is valid and the                following shall be performed:                -   a. Compute the x-component of target velocity (Vxt)                    and the y-component of target velocity (Vyt):

$\begin{matrix}{{V\; x\; t} = \frac{{{R2}\;\sin\;{B2}} + {X\;{s2}} - {{R1}\;\sin\;{B1}} - {X\;{s1}}}{{t2} - {t1}}} & (87) \\{{V\; y\; t} = \frac{{{R2}\;\cos\;{B2}} + {Y\;{s2}} - {{R1}\;\cos\;{B1}} - {Y\;{s1}}}{{t2} - {t1}}} & (88)\end{matrix}$

-   -   -   -   -   b. Compute the frequency at t_(i) with respect to                    the sensor associated with the ith measurement                    (f_(i)):

$\begin{matrix}{f_{i} = {F\; b\frac{{c\; R\; s_{i}} + {V\; x\; s_{i}R\; x_{i}} + {V\; y\; s_{i}R\; y_{i}}}{{c\; R\; s_{i}} + {V\; x\; t\; R\; x_{i}} + {V\; y\; t\; R\; y_{i}}}}} & (89)\end{matrix}$

-   -   -   -   -    where c is the average speed of sound.                -   c. Compute the frequency residual (RESf_(i)):                    RESf _(i) =fm _(i) −f _(i)  (90)                -    where fm_(i) is the measured frequency at t_(i).                -   d. Compute the normalized frequency residual (                    RESf_(i) ):

$\begin{matrix}{{\overset{\_}{RESf}}_{i} = \frac{{RESf}_{i}}{\sigma\; f_{i}}} & (91)\end{matrix}$

-   -   -   -   -    where σf_(i) is the standard deviation of the                    measured frequency at t_(i).

    -   c. If a range constraint is being imposed, then the following        processing shall be performed:        -   i. Compute the range residual (RESr):            RESr=Rc−R  (92)        -    where Rc is the assumed target range.        -   ii. Compute the normalized range residual ( RESr):

$\begin{matrix}{\overset{\_}{RESr} = \frac{RESr}{\sigma\; R}} & (93)\end{matrix}$where σR is the assumed target range standard deviation.

-   -   d. If a speed constraint is being imposed, then the following        processing shall be performed:        -   i. Compute the x-component of target velocity (Vxt) and the            y-component of target velocity (Vyt):

$\begin{matrix}{{V\; x\; t} = \frac{{{R2}\;\sin\;{B2}} + {X\;{s2}} - {{R1}\;\sin\;{B1}} - {X\;{s1}}}{{t2} - {t1}}} & (94) \\{{V\; y\; t} = \frac{{{R2}\;\cos\;{B2}} + {Y\;{s2}} - {{R1}\;\cos\;{B1}} - {Y\;{s1}}}{{t2} - {t1}}} & (95)\end{matrix}$

-   -   -   ii. Compute the target speed (V):            V=√{square root over (Vxt²+Vyt²)}  (96)        -   iii. Compute the speed residual (RESv):            RESv=Vc−V  (97)        -    where V is the assumed target speed.        -   iv. Compute the normalized speed residual ( RESv):

$\begin{matrix}{\overset{\_}{R\; E\; S\; v} = \frac{R\; E\; S\; v}{\sigma\; V}} & (98)\end{matrix}$

-   -   -    where σV is the assumed target speed standard deviation.

    -   e.) Compute the Endpoint coordinate performance index as the        square root of the means of the squared normalized residuals.        7. Set the minimum and maximum range at t1 with respect to the        sensor associated with time line 1 (R1 _(min),R1 _(max)) and the        minimum and maximum range at t2 with respect to the sensor        associated with time line 2 (R2 _(min),R2 _(max)) as follows:

    -   a. If the measurement at time line 1 is a bearing, set the        minimum range at t1 with respect to the sensor associated with        time line 1 (R1 _(min)) to the minimum range constraint which is        defaulted to 100.

    -   b. If the measurement at time line 1 is a conical angle, compute        the minimum range at t1 with respect to the sensor associated        with time line 1 (R1 _(min)). If R1 _(min) is less than the        minimum range constraint, set R1 _(min) to the minimum range        constraint which is defaulted to 100. The minimum range with        respect to the sensor is computed as described in equations (4)        thru (8).

    -   c. Set the maximum range at t1 with respect to the sensor        associated with time line 1 (R1 _(max)) to the maximum range        constraint with is defaulted to 200000.

    -   d. If the measurement at time line 2 is a bearing, set the        minimum range at t2 with respect to the sensor associated with        time line 2 (R2 _(min)) to the minimum range constraint which is        defaulted to 100.

    -   e. If the measurement at time line 2 is a conical angle, compute        the minimum range at t2 with respect to the sensor associated        with time line 2 (R2 _(min)). If R2 _(min) is less than the        minimum range constraint, set R2 _(min) to the minimum range        constraint which is defaulted to 100. The minimum range with        respect to the sensor is computed as described in equations (4)        thru (8).

    -   f. Set the maximum range at t2 with respect to the sensor        associated with time line 2 (R2 _(max)) to the maximum range        constraint which is defaulted to 200000.        8. Compute the endpoint parameters as follows:

    -   a. If the measurement at time line 1 is a bearing, set true        bearing at t1 with respect to the sensor associated with time        line 1 (B1) to the smoothed bearing estimate at time line 1        output by the endpoint smoother algorithm.

    -   b. If the measurement at time line 1 is a conical angle,        -   i. Compute the target image depth at t1 with respect to the            sensor associated with time line 1 (Rz1) as described for            equations (4) and (5).        -   ii. Compute the maximum depression/elevation (D/E) angle at            t1 with respect to the sensor associated with time line 1            (θ1 _(max)) as described for equations (6) thru (8).        -   iii. Compute the slant range at t1 with respect to the            sensor associated with time line 1 (Rs1):            Rs1=√{square root over (R1²+Rz1²)}  (99)        -   iv. Compute the D/E angle t1 with respect to the sensor            associated with time line 1 (θ1):

$\begin{matrix}{{\theta\; 1} = {\sin^{- 1}\left( \frac{R\;{z1}}{R\;{s1}} \right)}} & (100)\end{matrix}$

-   -   -   v. If θ>θ1 _(max), the D/E angle is invalid and processing            shall terminate.        -   vi. Compute the cosine of relative bearing at t1 with            respect to the sensor associated with time line 1 (cBr1) as            follows:

$\begin{matrix}{{c\;{Br1}} = \frac{{\cos\;{\beta 1}} + {\sin\; C\;{s1}\;\sin\;\theta\; 1}}{\cos\; C\;{s1cos}\;\theta\; 1}} & (101)\end{matrix}$

-   -   -    where Cs1 is the cant angle at t1 of the sensor associated            with time line 1        -   β1 is the smoothed conical angle estimate at time line 1            output by the endpoint smoother algorithm.        -   vii. Insure that −0.99999<cBr1<0.99999.        -   viii. Compute the relative bearing at t1 with respect to the            sensor associated with time line 1 (Br1) as follows:            Br1=cos⁻¹ cBr1  (102)        -   ix. If the port/starboard assumption for time line 1            indicates port, set Br1=2π−Br1.        -   x. Compute the true bearing at t1 with respect to the sensor            associated with time line 1 (B1) as follows:            B1=Br1+Hs1  (103)        -    where Hs1 is the heading at t1 of the sensor associated            with time line 1

    -   c. If the measurement at time line 2 is a bearing, set true        bearing at t2 with respect to the sensor associated with time        line 2 (B2) to the smoothed bearing estimate at time line 2        output by the endpoint smoother algorithm.

    -   d. If the measurement at time line 2 is a conical angle,        -   i. Compute the target image depth at t2 with respect to the            sensor associated with time line 2 (Rz2) as described for            equations (4) and (5).        -   ii. Compute the maximum D/E angle at t2 with respect to the            sensor associated with time line 2 (θ2 _(max)) as described            for equations (6) thru (8).        -   iii. Compute the slant range at t2 with respect to the            sensor associated with time line 2 (Rs2):

$\begin{matrix}{{R\;{s2}} = \sqrt{{R2}^{2} + {R\;{z2}^{2}}}} & (104)\end{matrix}$

-   -   -   iv. Compute the D/E angle at t2 with respect to the sensor            associated with time line 2 (θ₂):

$\begin{matrix}{{\theta\; 2} = {\sin^{- 1}\left( \frac{R\;{z2}}{R\;{s2}} \right)}} & (105)\end{matrix}$

-   -   -   v. If θ2>θ2 _(max), the D/E angle is invalid and processing            shall terminate.        -   vi. Compute the cosine of relative bearing at t2 with            respect to the sensor associated with time line 2 (cBr2) as            follows:

$\begin{matrix}{{c\; B\;{r2}} = \frac{{\cos\;\beta\; 2} + {\sin\; C\;{s2}\;\sin\;\theta\; 2}}{\cos\; C\;{s2}\;\cos\;\theta\; 2}} & (106)\end{matrix}$

-   -   -    where Cs2 is the cant angle at t2 of the sensor associated            with time line 2.        -   β2 is the smoothed conical angle estimate at time line 2            output by the endpoint smoother algorithm.        -   vii. Insure that −0.99999<cBr2<0.99999.        -   viii. Compute the relative bearing at t2 with respect to the            sensor associated with time line 2 (Br2) as follows:            Br2=cos⁻¹ cBr2  (107)        -   ix. If the port/starboard assumption for time line 2            indicates port, set Br2=2π−Br2.        -   x. Compute the true bearing at t2 with respect to the sensor            associated with time line 2 (B2) as follows:            B2=Br2+Hs2  (108)        -    where Hs2 is the heading at t2 of the sensor associated            with time line 2.            9. Compute the initial x-component of target velocity (Vxt)            and initial y-component of target velocity (Vyt):

$\begin{matrix}{{V\; x\; t} = \frac{{{R2}\;\sin\;{B2}} + {X\;{s2}} - {{R1}\;\sin\;{B1}} - {X\;{s1}}}{{t2} - {t1}}} & (109) \\{{V\; y\; t} = \frac{{{R2}\;\cos\;{B2}} + {Y\;{s2}} - {{R1}\;\cos\;{B1}} - {Y\;{s1}}}{{t2} - {t1}}} & (110)\end{matrix}$where Xs2 is the x-coordinate of the position at t2 of the sensorassociated with time line 2

-   -   Ys2 is the y-coordinate of the position at t2 of the sensor        associated with time line 2    -   Xs1 is the x-coordinate of the position at t1 of the sensor        associated with time line 1    -   Ys1 is the y-coordinate of the position at t1 of the sensor        associated with time line 1        10. Compute the initial target course (Ct) and speed (Vt)        estimates:

$\begin{matrix}{{C\; t} = {\tan^{- 1}\left( \frac{V\; x\; t}{{V\; y\; t}\;} \right)}} & (111)\end{matrix}$Vt=√{square root over (Vxt²+Vyt²)}  (112)11. Compute initial x-coordinate of target position at tc (Xtc) andinitial y-coordinate of target position at tc (Ytc):Xtc=R2 sin B2+Xs2+Vxt(t2−tc)  (113)Ytc=R2 cos B2+Ys2+Vyt(t2−tc)  (114)

where tc is current time

12. Compute initial x-component of range at tc with respect to own ship(Rxoc) and initial y-component of range at tc with respect to own ship(Ryoc):Rxoc=Xtc−Xoc  (115)Ryoc=Ytc−Yoc  (116)

where Xoc is the x-coordinate of own ship position at tc

Yoc is the y-coordinate of own ship position at tc

13. Compute initial range at tc with respect to own ship (Roc) and truebearing at tc with respect to own ship (Boc):Roc=√{square root over (Rxoc²+Ryoc²)}  (117)

$\begin{matrix}{{B\; o\; c}\; = {\tan^{- 1}\left( \frac{R\; x\; o\; c}{R\; y\; o\; c} \right)}} & (118)\end{matrix}$14. If a range constraint is being imposed, limit the initial range attc with respect to own ship to the maximum target range constraint. If aspeed constraint is being imposed, limit the initial target speedestimate (Vt) to the maximum target speed constraint.15. Gauss-Newton iterations shall be performed as described inparagraphs a through r below, until the algorithm converges as describedin paragraph r or until twenty-five iterations have been performed.

-   -   a. If the measurement at time line 1 is a conical angle, compute        endpoint parameters at the time of the measurement at time line        1:        -   i. Limit the range at t1 with respect to the sensor            associated with time line 1 (R1) to a minimum of R1            _(min)+0.1.        -   ii. Compute the target image depth at t1 with respect to the            sensor associated with time line (Rz1) as described for            equations (4) and (5).        -   iii. Compute the maximum depression/elevation (D/E) angle at            t1 with respect to the sensor associated with time line 1            (θ1 _(max)) as described for equations (6) thru (8).        -   iv. Compute the slant range at t1 with respect to the sensor            associated with time line 1 (Rs1):            Rs1=√{square root over (R1²+Rz1²)}  (119)        -   v. Compute the D/E angle at t1 with respect to the sensor            associated with time line 1 (θ1):

$\begin{matrix}{{\theta\; 1} = {\sin^{- 1}\left( \frac{R\;{z1}}{R\;{s1}} \right)}} & (120)\end{matrix}$

-   -   -   vi. If θ1<θ1 _(max), perform the following:            -   1.) Compute the cosine of relative bearing at t1 with                respect to the sensor associated with time line 1 (cBr1)                as follows:

$\begin{matrix}{{c\; B\;{r1}} = \frac{{\cos\;\beta\; 1} + {\sin\; C\;{s1}\;\sin\;\theta\; 1}}{\cos\; C\;{s1}\;\cos\;\theta\; 1}} & (121)\end{matrix}$

-   -   -   -   2.) Insure that −0.99999<cBr1<0.99999.            -   3.) Compute the relative bearing at t1 with respect to                the sensor associated with time line (Br1) as follows:                Br1=cos⁻¹ cBr1  (122)            -   4.) If the port/starboard assumption for time line 1                indicates port, set Br1=2π−Br1.            -   5.) Compute the true bearing at t1 with respect to the                sensor associated with time line 1 (B1) as follows:                B1=Br1+Hs1  (123)            -   6.) Compute the slant range at t1 respect to the sensor                associated with time line 1 (Rs1) as follows:                Rs1=√{square root over (R1²+Rz1²)}  (124)

    -   vii. If θ1>θ1 _(max), terminate all processing.

    -   b. If the measurement at time line 2 is a conical angle, compute        endpoint parameters at the time of the measurement at time line        2:        -   i. Limit the range at t2 with respect to the sensor            associated with time line 2 (R2) to a minimum of R2            _(min)+0.1.        -   ii. Compute the target image depth at t2 with respect to the            sensor associated with time line 2 (Rz2) as described for            equations (4) and (5).        -   iii. Compute the maximum D/E angle at t2 with respect to the            sensor associated with time line 2 (θ2 _(max)) as described            for equations (6) thru (8).        -   iv. Compute the slant range at t2 with respect to the sensor            associated with time line 2 (Rs2):            Rs2=√{square root over (R2²+Rz2²)}  (125)        -   v. Compute the D/E angle at t2 with respect to the sensor            associated with time line 2 (θ2):

$\begin{matrix}{{\theta\; 2} = {\sin^{- 1}\left( \frac{R\;{z2}}{R\;{s2}} \right)}} & (126)\end{matrix}$

-   -   -   vi. If θ2<θ2 _(max), perform the following:            -   1.) Compute the cosine of relative bearing at t2 with                respect to the sensor associated with time line 2 (cBr2)                as follows:

$\begin{matrix}{{cBr2} = \frac{{\cos\;{\beta 2}} + {\sin\;{Cs2}\;\sin\;{\theta 2}}}{\cos\;{Cs2}\;\cos\;{\theta 2}}} & (127)\end{matrix}$

-   -   -   -   2.) Insure that −0.99999<cBr2<0.99999.            -   3.) Compute the relative bearing at t2 with respect to                the sensor associated with time line 2 (Br2) as follows:                Br2=cos⁻¹ cBr2  (128)            -   4.) If the port/starboard assumption for time line 2                indicates port, set Br2=2π−Br2.            -   5.) Compute the true bearing at t2 with respect to the                sensor associated with time line 2 (B2) as follows:                B2=Br2+Hs2  (129)            -   6.) Compute the slant range at t2 respect to the sensor                associated with time line 2 (Rs2) as follows:                Rs2=√{square root over (R2²+Rz2²)}  (130)

        -   vii. If θ2>θ2 _(max), terminate all processing.

    -   c. For each measurement in the batch:        -   i. Compute the x-component of range at t_(i) with respect to            the sensor associated with the ith measurement (Rx_(i)) and            the y-component of range at t_(i) with respect to the sensor            associated with the ith measurement (Ry_(i)):

$\begin{matrix}{{T1}_{i} = \frac{t_{i} - {t1}}{{t2} - {t1}}} & (131)\end{matrix}$T2_(i)=1−T1₁  (132)Rx _(i) =T2_(i) R1 sin B1+T1_(i) R2 sin B2+T1_(i)(Xs2−Xs1)−(Xs _(i)−Xs1)  (133)Ry _(i) =T2_(i) R1 cos B1+T1_(i) R2 cos B2+T1_(i)(Ys2−Ys1)−(Ys _(i)−Ys1)  (134)

-   -   -    where Xs_(i) is the x-coordinate of the position at t_(i)            of the sensor associated with the ith measurement        -   Ys_(i) is the y-coordinate of the position at t_(i) of the            sensor associated with the ith measurement t_(i) is the time            of the ith measurement        -   ii. Compute the range at t_(i) with respect to the sensor            associated with the ith measurement (R_(i)) and bearing at            t_(i) with respect to the sensor associated with the ith            measurement (B_(i)):

$\begin{matrix}{R_{i} = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2}}} & (135) \\{B_{i} = {\tan^{- 1}\left( \frac{{Rx}_{i}}{{Ry}_{i}} \right)}} & (136)\end{matrix}$

-   -   -   iii. Compute the target image depth at t_(i) with respect to            the sensor associated with the ith measurement (Rz_(i)) and            D/E angle at t_(i) with respect to the sensor associated            with the ith measurement (θ_(i)) as described for            equations (75) and (76).        -   iv. If the measurement at time line 1 is a bearing, the            following shall be performed:            -   1.) Compute the partial derivative of the x-component of                target range at t_(i) with respect to the sensor                associated with the ith measurement with respect to                range at t1 with respect to the sensor associated with                time line 1

$\left( \frac{\partial{Rx}_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the y-component of target                range at t_(i) with respect to range at t1 with respect                to the sensor associated with line 1

$\begin{matrix}{\left( \frac{\partial{Ry}_{i}}{\partial{R1}} \right):} & \; \\{\frac{\partial{Rx}_{i}}{\partial{R1}} = {{T2}_{i}\sin\;{B1}}} & (137) \\{\frac{\partial{Ry}_{i}}{\partial{R1}} = {{T2}_{i}\cos\;{B1}}} & (138)\end{matrix}$

-   -   -   -   2.) Compute the partial derivative of target horizontal                range at t_(i) with respect to the sensor associated                with the ith measurement with respect to range at t1                with respect to the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial R_{i}}{\partial{R1}} \right):} & \mspace{11mu} \\{\frac{\partial R_{i}}{\partial{R1}} = \frac{{{Rx}_{i}\frac{\partial{Rx}_{i}}{\partial{R1}}} + {{Ry}_{i}\frac{\partial{Ry}_{i}}{\partial{R1}}}}{R_{i}}} & (139)\end{matrix}$

-   -   -   -   3.) Compute the partial derivative of the bearing at                t_(i) with respect to the sensor associated with the ith                measurement with respect to range at t1 with respect to                the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial B_{i}}{\partial{R1}} \right):} & \mspace{11mu} \\{\frac{\partial B_{i}}{\partial{R1}} = \frac{{T2}_{i}{\sin\left( {{B1} - B_{i}} \right)}}{R_{i}}} & (140)\end{matrix}$

-   -   -   -   4.) Compute the partial derivative of the sine of true                bearing at t1 with respect to the sensor associated with                time line 1 with respect to range at t1 with respect to                the sensor associated with time line 1

$\left( \frac{\partial{sB1}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the cosine of true                bearing at t1 with respect to the sensor associated with                time line 1 with respect to range at t1 with respect to                the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial{cB1}}{\partial{R1}} \right):} & \; \\{\frac{\partial{sB1}}{\partial{R1}} = 0} & (141) \\{\frac{\partial{cB1}}{\partial{R1}} = 0} & (142)\end{matrix}$

-   -   -   v. If the measurement at time line 1 is a conical angle, the            following shall be performed:            -   1.) Compute the partial derivative of the sine of true                bearing at t1 with respect to the sensor associated with                time line 1 with respect to range at t1 with respect to                the sensor associated with time line 1

$\left( \frac{\partial{sB1}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the cosine of true                bearing at t1 with respect to the sensor associated with                time line 1 with respect to range at t1 with respect to                the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial{cB1}}{\partial{R1}} \right):} & \; \\{{TMP1} = \frac{- {Rz1}}{{R1}^{2}\cos\;{{Cs1}\left( {\frac{{Rz1}\;\cos\;{\beta 1}}{Rs1} + {\sin\;{Cs1}}} \right)}}} & (143) \\{{TMP2} = {{- \left( \frac{\cos\;{Br1}}{\sin\;{Br1}} \right)}{TMP1}}} & (144) \\{\frac{\partial{cB1}}{\partial{R1}} = {{{TMP1}\;\cos\;{Hs1}} - {{TMP}\;\sin\;{Hs1}}}} & (145) \\{\frac{\partial{sB1}}{\partial{R1}} = {{- \left( \frac{\cos\;{B1}}{\sin\;{B1}} \right)}\frac{\partial{cB1}}{\partial{R1}}}} & (146)\end{matrix}$

-   -   -   -   2.) Compute the partial derivative of the x-component of                range at t_(i) with respect to the sensor associated                with the ith measurement with respect to range at t1                with respect to the sensor associated with time line 1

$\left( \frac{\partial{Rx}_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the y-component of range                at t_(i) with respect to the sensor associated with the                ith measurement with respect to range at t1 with respect                to the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial{Ry}_{i}}{\partial{R1}} \right):} & \; \\{\frac{\partial{Rx}_{i}}{\partial{R1}} = {{T2}_{i}\left( {{{R1}\frac{\partial{sB1}}{\partial{R1}}} + {\sin\;{B1}}} \right)}} & (147) \\{\frac{\partial{Ry}_{i}}{\partial{R1}} = {{T2}_{i}\left( {{{R1}\frac{\partial{cB1}}{\partial{R1}}} + {\cos\;{B1}}} \right)}} & (148)\end{matrix}$

-   -   -   -   3.) Compute the partial derivative of horizontal range                at t_(i) with respect to the sensor associated with the                ith measurement with respect to the range at t1 with                respect to the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial R_{i}}{\partial{R1}} \right):} & \; \\{\frac{\partial R_{i}}{\partial{R1}} = \frac{{{Rx}_{i}\frac{\partial{Rx}_{i}}{\partial{R1}}} + {{Ry}_{i}\frac{\partial{Ry}_{i}}{\partial{R1}}}}{R_{i}}} & (147)\end{matrix}$

-   -   -   -   4.) Compute the partial derivative of true bearing at                t_(i) with respect to the sensor associated with the ith                measurement with respect to range at t1 with respect to                the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial B_{i}}{\partial{R1}} \right):} & \; \\{\frac{\partial B_{i}}{\partial{R1}} = \frac{{{Ry}_{i}\frac{\partial{Rx}_{i}}{\partial{R1}}} - {{Rx}_{i}\frac{\partial{Ry}_{i}}{\partial{R1}}}}{R_{i}^{2}}} & (150)\end{matrix}$

-   -   -   vi. If the measurement at time line 2 is a bearing, the            following shall be performed:            -   1.) Compute the partial derivative of the x-component of                target range at t_(i) with respect to the sensor                associated with the ith measurement with respect to                range at t2 with respect to the sensor associated with                time line 2

$\left( \frac{\partial{Rx}_{i}}{\partial{R2}} \right)$

-   -   -   -    and the partial derivative of the y-component of target                range at t_(i) with respect to the sensor associated                with the ith measurement with respect to range at t2                with respect to the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{Ry}_{i}}{\partial{R2}} \right):} & \; \\{\frac{\partial{Rx}_{i}}{\partial{R2}} = {{T1}_{i}\sin\;{B2}}} & (151) \\{\frac{\partial{Ry}_{i}}{\partial{R2}} = {{T1}_{i}\cos\;{B2}}} & (152)\end{matrix}$

-   -   -   -   2.) Compute the partial derivative of target horizontal                range at t_(i) with respect to the sensor associated                with the ith measurement with respect to range at t2                with respect to the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial R_{i}}{\partial{R2}} \right):} & \; \\{\frac{\partial R_{i}}{\partial{R2}} = \frac{{{Rx}_{i}\frac{\partial{Rx}_{i}}{\partial{R2}}} + {{Ry}_{i}\frac{\partial{Ry}_{i}}{\partial{R2}}}}{R_{i}}} & (153)\end{matrix}$

-   -   -   -   3.) Compute the partial derivative of the bearing at                t_(i) with respect to the sensor associated with the ith                measurement with respect to range at t2 with respect to                the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial B_{i}}{\partial{R2}} \right):} & \; \\{\frac{\partial B_{i}}{\partial{R2}} = \frac{{T1}_{i}{\sin\left( {{B2} - B_{i}} \right)}}{R_{i}}} & (154)\end{matrix}$

-   -   -   -   4.) Compute the partial derivative of the sine of true                bearing at t2 with respect to the sensor associated with                time line 2 with respect to range at t2 with respect to                the sensor associated with time line 2

$\left( \frac{\partial{sB2}}{\partial{R2}} \right)$

-   -   -   -    and the partial derivative of the cosine of true                bearing at t2 with respect to the sensor associated with                time line 2 with respect to range at t2 with respect to                the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{cB2}}{\partial{R2}} \right):} & \; \\{\frac{\partial{sB2}}{\partial{R2}} = 0} & (155) \\{\frac{\partial{cB2}}{\partial{R2}} = 0} & (156)\end{matrix}$

-   -   -   vii. If the measurement at time line 2 is a conical angle,            the following shall be performed:            -   1.) Compute the partial derivative of the sine of true                bearing at t2 with respect to the sensor associated with                time line 2 with respect to range at t2 with respect to                the sensor associated with time line 2

$\left( \frac{\partial{sB2}}{\partial{R2}} \right)$

-   -   -   -    and the partial derivative of the cosine of true                bearing at t2 with respect to the sensor associated with                time line 2 with respect to range at t2 with respect to                the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{cB2}}{\partial{R2}} \right):} & \; \\{{TMP1} = \frac{- {Rz2}}{{R2}^{2}\cos\;{{Cs2}\left( {\frac{{Rz2}\;\cos\;{\beta 2}}{Rs2} + {\sin\;{Cs2}}} \right)}}} & (157) \\{{TMP2} = {{- \left( \frac{\cos\;{Br2}}{\sin\;{Br2}} \right)}{TMP1}}} & (158) \\{\frac{\partial{cB2}}{\partial{R2}} = {{{TMP1}\;\cos\;{Hs2}} - {{TMP2}\;\sin\;{Hs2}}}} & (159) \\{\frac{\partial{sB2}}{\partial{R2}} = {{- \left( \frac{\cos\;{B2}}{\sin\;{B2}} \right)}\frac{\partial{cB2}}{\partial{R2}}}} & (160)\end{matrix}$

-   -   -   -   2.) Compute the partial derivative of the x-component of                range at t_(i) with respect to the sensor associated                with the ith measurement with respect to range at t2                with respect to the sensor associated with time line 2

$\left( \frac{\partial{Rx}_{i}}{\partial{R2}} \right)$

-   -   -   -    and the partial derivative of the y-component of range                at t_(i) with respect to the sensor associated with the                ith measurement with respect to range at t2 with respect                to the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{Ry}_{i}}{\partial{R2}} \right):} & \; \\{\frac{\partial{Rx}_{i}}{\partial{R2}} = {{T1}_{i}\left( {{{R2}\frac{\partial{sB2}}{\partial{R2}}} + {\sin\;{B2}}} \right)}} & (161) \\{\frac{\partial{Ry}_{i}}{\partial{R2}} = {{T1}_{i}\left( {{{R2}\frac{\partial{cB2}}{\partial{R2}}} + {\cos\;{B2}}} \right)}} & (162)\end{matrix}$

-   -   -   -   3.) Compute the partial derivative of horizontal range                at t_(i) with respect to the sensor associated with the                ith measurement with respect to the range at t2 with                respect to the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial R_{i}}{\partial{R2}} \right):} & \; \\{\frac{\partial R_{i}}{\partial{R2}} = \frac{{{Rx}_{i}\frac{\partial{Rx}_{i}}{\partial{R2}}} + {{Ry}_{i}\frac{\partial{Ry}_{i}}{\partial{R2}}}}{R_{i}}} & (163)\end{matrix}$

-   -   -   -   4.) Compute the partial derivative of true bearing at                t_(i) with respect to the sensor associated with the ith                measurement with respect to range at t2 with respect to                the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial B_{i}}{\partial{R2}} \right):} & \; \\{\frac{\partial B_{i}}{\partial{R2}} = \frac{{{Ry}_{i}\frac{\partial{Rx}_{i}}{\partial{R2}}} - {{Rx}_{i}\frac{\partial{Ry}_{i}}{\partial{R2}}}}{R_{i}^{2}}} & (164)\end{matrix}$

-   -   -   viii. If the ith measurement is a bearing, then the            following shall be performed:            -   1.) Compute the bearing residual (RESb_(i)) such that                −π≦RESb_(i)≦π:                RESb _(i) =Bm _(i) −B _(i)  (165)            -    where Bm_(i) is the measured bearing at t_(i)            -   2.) Compute the normalized bearing residual ( RESb_(i) )                and normalized partial derivatives

$\begin{matrix}{\left( \overset{\_}{\frac{\partial B_{i}}{\partial{R1}},\frac{\partial B_{i}}{\partial{R2}}} \right):} & \; \\{\overset{\_}{{RESb}_{i}} = \frac{{RESb}_{i}}{\sigma\; b_{i}}} & (166) \\{\overset{\_}{\frac{\partial B_{i}}{\partial{R1}}} = \frac{\frac{\partial B_{i}}{\partial{R1}}}{\sigma\; b_{i}}} & (167) \\{\overset{\_}{\frac{\partial B_{i}}{\partial{R2}}} = \frac{\frac{\partial B_{i}}{\partial{R2}}}{\sigma\; b_{i}}} & (168)\end{matrix}$

-   -   -   -    where σb_(i) is the standard deviation of the bearing                measurement            -   3.) If frequency data are not being processed, set the                next row of the augmented Jacobian H Matrix as follows:

$\begin{matrix}\left\lbrack {\overset{\_}{\frac{\partial B_{i}}{\partial{R1}}}\mspace{14mu}\overset{\_}{\frac{\partial B_{i}}{\partial{R2}}}\mspace{14mu}\overset{\_}{{RESb}_{i}}} \right\rbrack & (169)\end{matrix}$

-   -   -   -   If frequency data are being processed, set the next row                of the augmented Jacobian H matrix as follows:

$\begin{matrix}\left\lbrack {\overset{\_}{\frac{\partial B_{i}}{\partial{R2}}}\mspace{14mu}\overset{\_}{\frac{\partial B_{i}}{\partial{R2}}}\mspace{14mu} O\mspace{14mu}\overset{\_}{{RESb}_{i}}} \right\rbrack & (170)\end{matrix}$

-   -   -   ix. If the ith measurement is a conical angle and the            D/E-mark indicates a valid D/E:            -   1.) Compute the true bearing at t_(i) with respect to                the sensor associated with the ith measurement (B_(i)):

$\begin{matrix}{B_{i} = {\tan^{- 1}\left( \frac{{Rx}_{i}}{{Ry}_{i}} \right)}} & (171)\end{matrix}$

-   -   -   -   2.) Compute the slant range at t_(i) with respect to the                sensor associated with the ith measurement (Rs_(i)):

$\begin{matrix}{{Rs}_{i} = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2} + {Rz}_{i}^{2}}} & (172)\end{matrix}$

-   -   -   -   3.) If the measurement at time line 1 is a conical                angle:                -   a Compute the partial derivative of slant range at                    t1 with respect to the sensor associated with time                    line 1 with respect to range at t1 with respect to                    the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial{Rs1}}{\partial{R1}} \right):} & \; \\{\frac{\partial{Rs1}}{\partial{R1}} = \frac{R1}{Rs1}} & (173)\end{matrix}$

-   -   -   -   -   b Compute the partial derivative of cosine of                    relative bearing at t1 with respect to the sensor                    associated with time line 1 with respect to range at                    t1 with respect to the sensor associated with time                    line

$\left( \frac{\partial{cBr1}}{\partial{R1}} \right)$

-   -   -   -   -    and the partial derivative of sine of relative                    bearing at t1 with respect to the sensor associated                    with time line 1 with respect to range at t1 with                    respect to the sensor associated with time line 1

$\begin{matrix}{\left( \frac{\partial{sBr1}}{\partial{R1}} \right)\text{:}} & \; \\{\frac{\partial{cBr1}}{\partial{R1}} = \frac{{\cos\;{{\beta 1}\left( {{{R1}\frac{\partial{Rs1}}{\partial{R1}}} - {Rs1}} \right)}} - {{Rz1}\;\sin\;{Cs1}}}{{R1}^{2}\cos\;{Cs1}}} & (174) \\{\frac{\partial{sBr1}}{\partial{R1}} = {{- \frac{\cos\;{Br1}}{\sin\;{Br1}}}\frac{\partial{cBr1}}{\partial{R1}}}} & (175)\end{matrix}$

-   -   -   -   4.) If the measurement at time line 2 is a conical                angle:                -   a Compute the partial derivative of slant range at                    t2 with respect to the sensor associated with time                    line 2 with respect to range at t2 with respect to                    the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{Rs2}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{\partial{Rs2}}{\partial{R2}} = \frac{R2}{Rs2}} & (176)\end{matrix}$

-   -   -   -   -   b Compute the partial derivative of cosine of                    relative bearing at t2 with respect to the sensor                    associated with time line 2 with respect to range at                    t2 with respect to the sensor associated with time                    line 2

$\left( \frac{\partial{cBr2}}{\partial{R2}} \right)$

-   -   -   -   -    and the partial derivative of sine of relative                    bearing at t2 with respect to the sensor associated                    with time line 2 with respect to range at t2 with                    respect to the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{sBr2}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{\partial{cBr2}}{\partial{R2}} = \frac{{\cos\;{{\beta r2}\left( {{{R2}\frac{\partial{Rs2}}{\partial{R2}}} - {Rs2}} \right)}} - {{Rz2}\;\sin\;{Cs2}}}{{R2}^{2}\cos\;{Cs2}}} & (177) \\{\frac{\partial{sBr2}}{\partial{R2}} = {{- \frac{\cos\;{Br2}}{\sin\;{Br2}}}\frac{\partial{cBr2}}{\partial{R2}}}} & (178)\end{matrix}$

-   -   -   -   5.) Compute the partial derivative of slant range at                t_(i) with respect to the sensor associated with the ith                measurement with respect to the x-component of range at                t_(i) with respect to the sensor associated with the ith                measurement

$\left( \frac{{\partial{Rs}_{i}}.}{\partial{Rx}_{i}} \right)$

-   -   -   -    and the partial derivative of slant range at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-component of range at                t_(i) with respect to the sensor associated with the ith                measurement

$\begin{matrix}{\left( \frac{\partial{Rs}_{i}}{\partial{Ry}_{i}} \right)\text{:}} & \; \\{\frac{\partial{Rs}_{i}}{\partial{Rx}_{i}} = \frac{{Rx}_{i}}{{Rs}_{i}}} & (179) \\{\frac{\partial{Rs}_{i}}{\partial{Ry}_{i}} = \frac{{Ry}_{i}}{{Rs}_{i}}} & (180)\end{matrix}$

-   -   -   -   6.) Compute the partial derivative of relative at t_(i)                with respect to the sensor associated with the ith                measurement with respect to range at t1 with respect to                the sensor associated with time line 1

$\left( \frac{\partial{Br}_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of relative bearing at t_(i)                with respect to the sensor associated with the ith                measurement with respect to range at t2 with respect to                the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial{Br}_{i}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{\partial{Br}_{i}}{\partial{R1}} = \frac{{{Ry}_{i}\frac{\partial{Rx}_{i}}{\partial{R1}}} - {{Rx}_{i}\frac{\partial{Ry}_{i}}{\partial{R1}}}}{R_{i}^{2}}} & (181) \\{\frac{\partial{Br}_{i}}{\partial{R2}} = \frac{{{Ry}_{i}\frac{\partial{Rx}_{i}}{\partial{R2}}} - {{Rx}_{i}\frac{\partial{Ry}_{i}}{\partial{R2}}}}{R_{i}^{2}}} & (182)\end{matrix}$

-   -   -   -   7.) Compute the partial derivative of D/E at t_(i) with                respect to the sensor associated with the ith                measurement with respect to range at t1 with respect to                the sensor associated with time line 1

$\left( \frac{\partial\theta_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of D/E angle at t_(i) with                respect to the sensor associated with the ith                measurement with respect to range at t2 with respect to                the sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial\theta_{i}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{\partial\theta_{i}}{\partial{R1}} = \frac{{- {Rz}_{i}}\frac{\partial R_{i}}{\partial{R1}}}{{Rs}_{i}^{2}}} & (183) \\{\frac{\partial\theta_{i}}{\partial{R2}} = \frac{{- {Rz}_{i}}\frac{\partial R_{i}}{\partial{R2}}}{{Rs}_{i}^{2}}} & (184)\end{matrix}$

-   -   -   -   8.) Compute the sine and cosine of D/E angle at t_(i)                with respect to the sensor associated with the ith                measurement:

$\begin{matrix}{{\sin\;\theta_{i}} = \frac{{Rz}_{i}}{{Rs}_{i}}} & (185) \\{{\cos\;\theta_{i}} = \frac{R_{i}}{{Rs}_{i}}} & (186)\end{matrix}$

-   -   -   -   9.) Compute the relative bearing at t_(i) with respect                to the sensor associated with the ith measurement                (Br_(i)):                Br _(i) =B _(i) −Hs _(i)  (187)            -   10.) Compute the conical angle at t_(i) from a                horizontal array with respect to the sensor associated                with the ith measurement (βh_(i)):                βh _(i)=cos⁻¹(cos θ_(i) cos Br _(i))  (188)            -   11.) Is sin βh_(i)≠0, compute the partial derivative of                the conical angle at t_(i) from a horizontal array with                respect to the sensor associated with the ith                measurement with respect to range at t1 with respect to                the sensor associated with time line 1

$\left( \frac{{\partial\beta}\; h_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the conical angle at                t_(i) from a horizontal array with respect to the sensor                associated with the ith measurement with respect to                range at t2 with respect to the sensor associated with                time line 2

$\begin{matrix}{\left( \frac{{\partial\beta}\; h_{i}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{{\partial\beta}\; h_{i}}{\partial{R1}} = \frac{{\cos\;{Br}_{i}\sin\;\theta_{i}\frac{\partial\theta_{i}}{\partial{R1}}} + {\sin\;{Br}_{i}\cos\;\theta_{i}\frac{\partial{Br}_{i}}{\partial{R1}}}}{\sin\;\beta\; h_{i}}} & (189) \\{\frac{{\partial\beta}\; h_{i}}{\partial{R2}} = \frac{{\cos\;{Br}_{i}\sin\;\theta_{i}\frac{\partial\theta_{i}}{\partial{R2}}} + {\sin\;{Br}_{i}\cos\;\theta_{i}\frac{\partial{Br}_{i}}{\partial{R2}}}}{\sin\;\beta\; h_{i}}} & (190)\end{matrix}$

-   -   -   -   12.) Compute the x-component of range at t_(i) with                respect to the sensor associated with the ith                measurement at the time of the ith measurement                (xta_(i)), the y-component of range at t_(i) with                respect to the sensor associated with the ith                measurement at the time of the ith measurement (yta_(i))                and the z-component of range at t_(i) with respect to                the sensor associated with the ith measurement at the                time of the ith measurement (zta_(i)) rotated to the                axis of the array:                xta _(i) =Rx _(i) cos Hs _(i) −Ry _(i) sin Hs                _(i)  (191)                yta _(i)=(Rx _(i) sin Hs _(i) +Ry _(i) cos Hs _(i))cos                Cs _(i) −Rz _(i) sin Cs _(i)  (192)                zta _(i)=(Rx _(i) sin Hs _(i) +Ry _(i) cos Hs _(i))sin                Cs _(i) −Rz _(i) cos Cs _(i)  (193)            -   13.) Compute the conical angle at t_(i) with respect to                the sensor associated with the ith measurement (β_(i)):            -   If yta_(i)≠0,

$\begin{matrix}{\beta_{i} = {\tan^{- 1}\left( \frac{\sqrt{{xta}_{i}^{2} + {zta}_{i}^{2}}}{{yta}_{i}} \right)}} & (194)\end{matrix}$

-   -   -   -   otherwise,

$\begin{matrix}{\beta_{i} = \frac{\pi}{2}} & (195)\end{matrix}$

-   -   -   -   14.) If sinβ_(i)≠0, compute the partial derivative of                the conical angle at t_(i) with respect to the sensor                associated with the ith measurement with respect to                range at t1 with respect to the sensor associated with                time line 1

$\left( \frac{\partial\beta_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the conical angle at                t_(i) with respect to range at t2 with respect to the                sensor associated with time line 2

$\begin{matrix}{\left( \frac{\partial\beta_{i}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{\partial\beta_{i}}{\partial{R1}} = \frac{{{\cos{Cs}}_{i}\sin\;\beta\; h_{i}\frac{{\partial\beta}\; h_{i}}{\partial{R1}}} + {\sin\;{Cs}_{i}\cos\;\theta_{i}\frac{\partial\theta_{i}}{\partial{R1}}}}{\sin\;\beta_{i}}} & (196) \\{\frac{\partial\beta_{i}}{\partial{R2}} = \frac{{{\cos{Cs}}_{i}\sin\;\beta\; h_{i}\frac{{\partial\beta}\; h_{i}}{\partial{R2}}} + {\sin\;{Cs}_{i}\cos\;\theta_{i}\frac{\partial\theta_{i}}{\partial{R2}}}}{\sin\;\beta_{i}}} & (197)\end{matrix}$

-   -   -   -   15.) Compute the conical angle residual (RESβ_(i)) such                that −π≦RESβ_(i)≦π:                RESβ _(i) =βm _(i)−β_(i)  (198)            -    where βm_(i) is the measured conical angle at t_(i).            -   16.) Compute the normalized conical angle residual                (RESβ_(i)) and normalized partial derivatives

$\begin{matrix}{\left( \overset{\_}{\frac{\partial\beta_{i}}{\partial{R1}},\frac{\partial\beta_{i}}{\partial{R2}}} \right)\text{:}} & \; \\{{\overset{\_}{RES\beta}}_{i} = \frac{{RES\beta}_{i}}{{\sigma\beta}_{i}}} & (199) \\{\frac{\overset{\_}{\partial\beta_{i}}}{\partial{R1}} = \frac{\frac{\partial\beta_{i}}{\partial{R1}}}{{\sigma\beta}_{i}}} & (200) \\{\frac{\overset{\_}{\partial\beta_{i}}}{\partial{R2}} = \frac{\frac{\partial\beta_{i}}{\partial{R2}}}{{\sigma\beta}_{i}}} & (201)\end{matrix}$

-   -   -   -    where σβ_(i) is the standard deviation of the conical                angle measurement.            -   17.) If frequency data are not being processed, set the                next row of the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial\beta_{i}}}{\partial{R1}}\frac{\overset{\_}{\partial\beta_{i}}}{\partial{R2}}\overset{\_}{{RES\beta}_{i}}} \right\rbrack & (202)\end{matrix}$

-   -   -   -   If frequency data are being processed, set the next row                of the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial\beta_{i}}}{\partial{R1}}\frac{\overset{\_}{\partial\beta_{i}}}{\partial{R2}}0\mspace{14mu}\overset{\_}{{RES\beta}_{i}}} \right\rbrack & (203)\end{matrix}$

-   -   -   x. If the ith measurement is a horizontal range:            -   1.) Compute the range residual (RESr_(i)):                RESr _(i) =Rm _(i) −R _(i)  (204)            -    where Rm_(i) is the measured horizontal range at t_(i).            -   2.) Compute the normalized range residual ( RESr_(i) )                and normalized partial derivatives

$\begin{matrix}{\left( \overset{\_}{\frac{\partial R_{i}}{\partial{R1}},\frac{\partial R_{i}}{\partial{R2}}} \right)\text{:}} & \; \\{\overset{\_}{{RESr}_{i}} = \frac{{RESr}_{i}}{{\sigma r}_{i}}} & (205) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{R1}} = \frac{\frac{\partial R_{i}}{\partial{R1}}}{{\sigma r}_{i}}} & (206) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{R2}} = \frac{\frac{\partial R_{i}}{\partial{R2}}}{{\sigma r}_{i}}} & (207)\end{matrix}$

-   -   -   -    where σr_(i) is the standard deviation of the range                measurement.            -   3.) If frequency data are not being processed, set the                next row of the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial R_{i}}}{\partial{R1}}\frac{\overset{\_}{\partial R_{i}}}{\partial{R2}}\overset{\_}{{RESr}_{i}}} \right\rbrack & (208)\end{matrix}$

-   -   -   -   If frequency data are being processed, set the next row                of the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial R_{i}}}{\partial{R1}}\frac{\overset{\_}{\partial R_{i}}}{\partial{R2}}0\mspace{14mu}\overset{\_}{{RESr}_{i}}} \right\rbrack & (209)\end{matrix}$

-   -   -   xi. If frequency data are being processed and the D/E mark            associated with the ith measurement indicates a valid D/E,            then the following shall be performed:            -   1.) Compute the x-component of target velocity (Vxt) and                the y-component of target velocity (Vyt):

$\begin{matrix}{{Vxt} = \frac{{{R2}\;\sin\;{B2}} + {Xs2} - {{R1}\;\sin\;{B1}} - {Xs1}}{{t2} - {t1}}} & (210) \\{{Vyt} = \frac{{{R2}\;\cos\;{B2}} + {Ys2} - {{R1}\;\cos\;{B1}} - {Ys1}}{{t2} - {t1}}} & (211)\end{matrix}$

-   -   -   -   2.) Compute the x-component of target velocity at t_(i)                relative to the sensor associated with the ith                measurement (Vx_(i)) and the y-component of target                velocity at t_(i) relative to the sensor associated with                the ith measurement (Vy_(i))                Vx _(i) =Vxt−Vxs _(i)  (212)                Vy _(i) =Vyt−Vys _(i)  (213)            -    where Vxs_(i) is the x-component of velocity of the                sensor associated with the ith measurement            -   Vys_(i) is the y-component of velocity of the sensor                associated with the ith measurement            -   3.) Compute the slant range at t_(i) with respect to the                sensor associated with the ith measurement (Rs_(i))

$\begin{matrix}{{Rs}_{i}\sqrt{R_{i}^{2} + {Rz}_{i}^{2}}} & (214)\end{matrix}$

-   -   -   -   4.) Compute the partial derivative of frequency at t_(i)                with respect to the sensor associated with the ith                measurement with respect to the x-component of range at                t_(i) with respect to the sensor associated with the ith                measurement

$\left( \frac{\partial f_{i}}{\partial{Rx}_{i}} \right)$

-   -   -   -    and the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-component of range at                t_(i) with respect to the sensor associated with the ith                measurement

$\begin{matrix}{\mspace{79mu}{\left( \frac{\partial f_{i}}{\partial{Ry}_{i}} \right)\text{:}}} & \; \\{\frac{\partial f_{i}}{\partial{Rx}_{i}} = {\frac{Fb}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)^{2}} \cdot \left( {{\left( {{{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)\left( {{\frac{c}{{Rs}_{i}}{Rx}_{i}} + {Vxs}_{i}} \right)} - {\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}} \right){Vx}_{i}}} \right)}} & (215) \\{\frac{\partial f_{i}}{\partial{Ry}_{i}} = {\frac{Fb}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)^{2}} \cdot \left( {{\left( {{{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)\left( {{\frac{c}{{Rs}_{i}}{Ry}_{i}} + {Vys}_{i}} \right)} - {\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}} \right){Vy}_{i}}} \right)}} & (216)\end{matrix}$

-   -   -   -   5.) Compute the partial derivative of frequency at t_(i)                with respect to the sensor associated with the ith                measurement with respect to the x-component of target                relative velocity at t_(i) with respect to the sensor                associated with the ith measurement

$\left( \frac{\partial f_{i}}{\partial{Vx}_{i}} \right)$

-   -   -   -    and the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-component of target                relative velocity at t_(i) with respect to the sensor                associated with the ith measurement

$\begin{matrix}{\mspace{79mu}{\left( \frac{\partial f_{i}}{\partial{Vy}_{i}} \right)\text{:}}} & \; \\{\frac{\partial f_{i}}{\partial{Vx}_{i}} = {\frac{Fb}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)^{2}} \cdot \left( {\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}} \right){Rx}_{i}} \right)}} & (217) \\{\frac{\partial f_{i}}{\partial{Vy}_{i}} = {\frac{Fb}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)^{2}} \cdot \left( {\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}} \right){Ry}_{i}} \right)}} & (218)\end{matrix}$

-   -   -   -   6.) Compute the partial derivative of the x-component of                target relative velocity at t_(i) with respect to the                sensor associated with the ith measurement with respect                to range at t1 with respect to the sensor associated                with the measurement at time line 1

$\left( \frac{\partial{Vx}_{i}}{\partial{R1}} \right)$

-   -   -   -    and the partial derivative of the y-component of target                relative velocity at t_(i) with respect to the sensor                associated with the ith measurement with respect to                range at t1 with respect to the sensor associated with                time line 1

$\begin{matrix}{\left( \frac{\partial{Vy}_{i}}{\partial{R1}} \right)\text{:}} & \; \\{\frac{\partial{Vx}_{i}}{\partial{R1}} = \frac{{{R1}\frac{{\partial\sin}\;{B1}}{\partial{R1}}} + {\sin\;{B1}}}{{t2} - {t1}}} & (219) \\{\frac{\partial{Vy}_{i}}{\partial{R1}} = \frac{{{R1}\frac{{\partial\cos}\;{B1}}{\partial{R1}}} + {\cos\;{B1}}}{{t2} - {t1}}} & (220)\end{matrix}$

-   -   -   -   7.) Compute the partial derivative of the x-component of                target relative velocity at t_(i) with respect to the                sensor associated with the ith measurement with respect                to range at t2 with respect to the sensor associated                with time line 2 at t2

$\left( \frac{\partial{Vx}_{i}}{\partial{R2}} \right)$

-   -   -   -    and the partial derivative of the y-component of target                relative velocity at t_(i) with respect to the sensor                associated with the ith measurement with respect to                range at t2 with respect to the sensor associated with                time line 2

$\begin{matrix}{\left( \frac{\partial{Vy}_{i}}{\partial{R2}} \right)\text{:}} & \; \\{\frac{\partial{Vx}_{i}}{\partial{R2}} = {- \frac{{{R2}\frac{\partial{sB2}}{\partial{R2}}} + {\sin\;{B2}}}{{t2} - {t1}}}} & (221) \\{\frac{\partial{Vy}_{i}}{\partial{R2}} = {- \frac{{{R2}\frac{\partial{cB2}}{\partial{R2}}} + {\cos\;{B2}}}{{t2} - {t1}}}} & (222)\end{matrix}$

-   -   -   -   8.) Compute the partial derivative of frequency at t_(i)                with respect to the sensor associated with the ith                measurement with respect to range at t1 with respect to                the sensor associated with time line 1

$\left( \frac{\partial f_{i}}{\partial{R1}} \right),$

-   -   -   -    the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to range at t2 with respect to                the sensor associated with time line 2

$\left( \frac{\partial f_{i}}{\partial{R2}} \right)$

-   -   -   -    and the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to base frequency

$\begin{matrix}{\left( \frac{\partial f_{i}}{\partial{Fb}} \right)\text{:}} & \; \\{\frac{\partial f_{i}}{\partial{R1}} = {{\frac{\partial f_{i}}{\partial{Rx}_{i}}\frac{\partial{Rx}_{i}}{\partial{R1}}} + {\frac{\partial f_{i}}{\partial{Ry}_{i}}\frac{\partial{Ry}_{i}}{\partial{R1}}} + {\frac{\partial f_{i}}{\partial{Vx}_{i}}\frac{\partial{Vx}_{i}}{\partial{R1}}} + {\frac{\partial f_{i}}{\partial{Vy}_{i}}\frac{\partial{Vy}_{i}}{\partial{R1}}}}} & (223) \\{\frac{\partial f_{i}}{\partial{R2}} = {{\frac{\partial f_{i}}{\partial{Rx}_{i}}\frac{\partial{Rx}_{i}}{\partial{R2}}} + {\frac{\partial f_{i}}{\partial{Ry}_{i}}\frac{\partial{Ry}_{i}}{\partial{R2}}} + {\frac{\partial f_{i}}{\partial{Vx}_{i}}\frac{\partial{Vx}_{i}}{\partial{R2}}} + {\frac{\partial f_{i}}{\partial{Vy}_{i}}\frac{\partial{Vy}_{i}}{\partial{R2}}}}} & (224) \\{\frac{\partial f_{i}}{\partial{Fb}} = \frac{{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)}} & (225)\end{matrix}$

-   -   -   -   9.) Compute the estimated frequency at t_(i) with                respect to the sensor associated with the ith                measurement at the time of the ith measurement:

$\begin{matrix}{f_{i} = {{Fb}\frac{{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}}{{cRs}_{i} + {VxtRx}_{i} + {VytRy}_{i}}}} & (226)\end{matrix}$

-   -   -   -   10.) Compute the frequency residual (RESf_(i)):                RESf _(i) =fm _(i) −f _(i)  (227)            -    where fm_(i) is the measured frequency at t_(i).            -   11.) Compute the normalized frequency residual (                RESf_(i) ) and normalized partial derivatives

$\begin{matrix}{\left( {\frac{\overset{\_}{\partial f_{i}}}{\partial{R1}},\frac{\overset{\_}{\partial f_{i}}}{\partial{R2}},\frac{\overset{\_}{\partial f_{i}}}{\partial{Fb}}} \right)\text{:}} & \; \\{\overset{\_}{{RESf}_{i}} = \frac{{RESf}_{i}}{{\sigma f}_{i}}} & (228) \\{\frac{\overset{\_}{\partial f_{i}}}{\partial{R1}} = \frac{\frac{\partial f_{i}}{\partial{R1}}}{{\sigma f}_{i}}} & (229) \\{\frac{\overset{\_}{\partial f_{i}}}{\partial{R2}} = \frac{\frac{\partial f_{i}}{\partial{R2}}}{{\sigma f}_{i}}} & (230) \\{\frac{\overset{\_}{\partial f_{i}}}{\partial{Fb}} = \frac{\frac{\partial f_{i}}{\partial{Fb}}}{{\sigma f}_{i}}} & (231)\end{matrix}$

-   -   -   -    where σf_(i) is the standard deviation of the frequency                measurement.            -   12.) Set the next row of the augmented Jacobian H matrix                to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial f_{i}}}{\partial{R1}}\frac{\overset{\_}{\partial f_{i}}}{\partial{R2}}\frac{\overset{\_}{\partial f_{i}}}{\partial{Fb}}\overset{\_}{{RESf}_{i}}} \right\rbrack & (232)\end{matrix}$

-   -   d. If a range constraint is being imposed, then the following        processing shall be performed:        -   i. Compute the range residual (RESR):            RESR=Rc−R  (233)        -    where Rc is the assumed target range.        -   ii. Compute the normalized range residual (RESR):

$\begin{matrix}{\overset{\_}{RESR} = \frac{RESR}{\sigma R}} & (234)\end{matrix}$

-   -   -    where σR is the assumed target range standard deviation.        -   iii. If frequency data are not being processed, set the next            row of the augmented Jacobian H Matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{{\partial R}\text{:}}}{\partial{R1}}\frac{\overset{\_}{{\partial R}\text{:}}}{\partial{R2}}\overset{\_}{RESR}} \right\rbrack & (235)\end{matrix}$

-   -   -   If frequency data are being processed, set the next row of            the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{{\partial R}\text{:}}}{\partial{R1}}\frac{\overset{\_}{{\partial R}\text{:}}}{\partial{R2}}0\mspace{14mu}\overset{\_}{RESR}} \right\rbrack & (236)\end{matrix}$

-   -   e. If a speed constraint is being imposed, then the following        processing shall be performed:        -   i. Compute the x-component of target velocity (Vxt) and the            y-component of target velocity (Vyt):

$\begin{matrix}{{Vxt} = \frac{{{R2}\;\sin\;{B2}} + {Xs2} - {{R1}\;\sin\;{B1}} - {Xs1}}{{t2} - {t1}}} & (237) \\{{Vyt} = \frac{{{R2}\;\cos\;{B2}} + {Ys2} - {{R1}\;\cos\;{B1}} - {Ys1}}{{t2} - {t1}}} & (238)\end{matrix}$

-   -   -   ii. Compute the partial derivative of the x-component of            target velocity with respect to range at t1 with respect to            the sensor associated with the measurement at time line 1

$\left( \frac{\partial{Vxt}}{\partial{R1}} \right)$

-   -   -    and the partial derivative of the y-component of target            velocity with respect to range t1 with respect to the sensor            associated with time line 1

$\begin{matrix}{\left( \frac{\partial{Vyt}}{\partial{R1}} \right)\text{:}} & \; \\{\frac{\partial{Vxt}}{\partial{R1}} = {- \frac{{{R1}\frac{\partial{sB1}}{\partial{R1}}} + {\sin\;{B1}}}{{t2} - {t1}}}} & (239) \\{\frac{\partial{Vxt}}{\partial{R1}} = {- \frac{{{R1}\frac{\partial{sB1}}{\partial{R1}}} + {\cos\;{B1}}}{{t2} - {t1}}}} & (240)\end{matrix}$

-   -   -   iii. Compute the partial derivative of the x-component of            target velocity with respect to range at t2 with respect to            the sensor associated with time line 2

$\left( \frac{\partial{Vxt}}{\partial{R2}} \right)$

-   -   -    and the partial derivative of the y-component of target            velocity with respect to range at t2 with respect to the            sensor associated with time line 2

$\begin{matrix}{\left( \frac{{\partial V}\; y\; t}{\partial{R2}} \right):} & \; \\{\frac{{\partial V}\; x\; t}{\partial{R2}} = {- \frac{{{R2}\frac{{{\partial s}\; B}\;}{\partial{R2}}} + {\sin\;{B2}}}{{t2} - {t1}}}} & (241) \\{\frac{{\partial V}\; y\; t}{\partial{R2}} = {- \frac{{{R2}\frac{{\partial c}\;{B2}}{\partial{R2}}} + {\cos\;{B2}}}{{t2} - {t1}}}} & (242)\end{matrix}$

-   -   -   iv. Compute the partial derivative of target speed with            respect to range at t1 with respect to the sensor associated            with time line 1

$\left( \frac{\partial v}{\partial{R1}} \right)$

-   -   -    and the partial derivative of target speed with respect to            range at t2 with respect to the sensor associated with time            line 2

$\begin{matrix}{\left( \frac{\partial v}{\partial{R2}} \right):} & \; \\{\frac{\partial v}{\partial{R1}} = \frac{{V\; x\; t\frac{{\partial V}\; x\; t}{\partial{R1}}} + {V\; y\; t\frac{{\partial V}\; y\; t}{\partial{R1}}}}{\sqrt{{V\; x\; t^{2}} + {V\; y\; t^{2}}}}} & (243) \\{\frac{\partial v}{\partial{R2}} = \frac{{V\; x\; t\frac{{\partial V}\; x\; t}{\partial{R2}}} + {V\; y\; t\frac{{\partial V}\; y\; t}{\partial{R2}}}}{\sqrt{{V\; x\; t^{2}} + {V\; y\; t^{2}}}}} & (244)\end{matrix}$

-   -   -   v. Compute the estimated target speed:            V=√{square root over (Vxt²+Vyt²)}  (245)        -   vi. Compute the speed residual (RESv):            RESv=Vc−V  (246)        -    where Vc is the assumed target speed.        -   vii. Compute the normalized speed residual ( RESv) and            normalized partial derivatives

$\begin{matrix}\left( {\frac{\overset{\_}{\partial v}}{\partial{R1}},\frac{\overset{\_}{\partial v}}{\partial{R2}}} \right) & \; \\{\overset{\_}{R\; E\; S\; v} = \frac{R\; E\; S\; v}{\sigma\; v}} & (247) \\{\frac{\overset{\_}{\partial v}}{\partial{R1}} = \frac{\frac{\partial v}{\partial{R1}}}{\sigma\; v}} & (248) \\{\frac{\overset{\_}{\partial v}}{\partial{R2}} = \frac{\frac{\partial v}{\partial{R2}}}{\sigma\; v}} & (249)\end{matrix}$

-   -   -    where σv is the standard deviation of the assumed target            speed.        -   viii. If frequency data are not being processed, set the            next row of the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial v}}{\partial{R1}}\frac{\overset{\_}{\partial v}}{\partial{R2}}\overset{\_}{R\; E\; S\; v}} \right\rbrack & (250)\end{matrix}$

-   -   -   If frequency data are being processed, set the next row of            the augmented Jacobian H matrix to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial v}}{\partial{R1}}\frac{\overset{\_}{\partial v}}{\partial{R2}}0\mspace{14mu}\overset{\_}{R\; E\; S\; v}} \right\rbrack & (251)\end{matrix}$

-   -   f. Reorder the rows of the H matrix such that a zero-valued        partial derivative does not appear along the diagonal.    -   g. Perform the Householder transformation on the m-by-ns+1        matrix H:        -   i. Compute values of s, μ, and β as:

$\begin{matrix}{s = {{- s}\; g\;{n\left( {H\left( {1,1} \right)} \right)}\left( {\sum\limits_{i = 1}^{m}\left\lbrack {H\left( {i,1} \right)}^{2} \right\rbrack} \right)}} & (252)\end{matrix}$u(1)=H(1,1)−s  (253)u(i)=H(i,1) i=2, . . . ,m  (254)

$\begin{matrix}{\beta = \frac{1}{s\;{u(1)}}} & (255)\end{matrix}$

-   -   -   ii. For j=2, . . . , ns+1, evaluate the following equations            (apply Householder transformation to the successive columns            of H):

$\begin{matrix}{\gamma = {\beta{\sum\limits_{i = 1}^{m}{{u(i)}{H\left( {i,j} \right)}}}}} & (256)\end{matrix}$H(i,j)=H(i,j)+γu(i)i=1, . . . , m  (257)

-   -   h. Extract the upper triangular matrix R from the upper left        hand corner of the transformed matrix H:        R(i,i)=H(i,i)i=1, . . . ,ns  (258)    -   i. Compute R⁻¹ by back substitution:        -   i. Compute R⁻¹ (1,1) as follows:

$\begin{matrix}{{R^{- 1}\left( {1,1} \right)} = \frac{1}{R\left( {1,1} \right)}} & (259)\end{matrix}$

-   -   -   ii. For j=2, . . . , ns perform the following:

$\begin{matrix}{{U\left( {j,j} \right)} = \frac{1}{R\left( {j,j} \right)}} & (260) \\{{{U\left( {k,j} \right)} = {{- \left( {\sum\limits_{l = k}^{j - 1}{{U\left( {k,l} \right)}{R\left( {l,j} \right)}}} \right)}{U\left( {j,j} \right)}}},{k = 1},\ldots\mspace{14mu},{j - 1}} & (261)\end{matrix}$

-   -   j. Set the Y vector to the last column of the transformed matrix        H:        Y(i)=H(i,n+1)i=1, . . . ,ns  (262)    -   k. Compute the gain vector (G):        G=R ⁻¹ Y  (263)    -   l. Determine if the gain vector is near zero. If both |G(1)| and        |G(2)| are less than or equal to 0.1, then the algorithm has        converged and Gauss-Newton iterations shall terminate, and        processing shall resume as described in paragraph g.    -   Otherwise, processing shall continue as described below.    -   m. Limit the range changes such that the updated range estimates        will be within bounds as follows:        -   i. If |G(1)|>10000 or |G(2)|>10000, perform the following            calculations up to twenty times which divide ΔR1 and ΔR2 by            2 until the updated range estimates will be within bounds:            α=1(Initialization)  (264)            R1_(temp) =R1+αG(1)  (265)            R2_(temp) =R2+αG(2)  (266)        -   If R1 _(min)<R1 _(temp)<R1 _(max) and R2 _(min)<R2            _(temp)<R2 _(max), update gain vector as follows:            G=αG  (267)        -    and continue as described in paragraph 16n below.            Otherwise, divide α by 2 and repeat the process.        -   ii. If |G(1)|<10000 or |G(2)|<10000, perform the following            calculations up to twenty times which decreases ΔR1 and ΔR2            by 5% until the updated range estimates will be within            bounds:            ii=0(Initialization)  (268)

$\begin{matrix}{\alpha = \frac{100 - {5i\; i} + 5}{100}} & (269)\end{matrix}$R1_(temp) =R1+αG(1)  (270)R2_(temp) =R2+αG(2)  (271)

-   -   -   If R1 _(min)<R1 _(temp)<R1 _(max) and R2 _(min)<R2            _(temp)<R2 _(max), update gain vector as follows:            G=αG  (272)        -    and continue as described in paragraph 16 below. Otherwise,            increase ii by 1 and repeat the process.

    -   n. Compute the stepsize (s) via the quadratic fit type line        search as follows:        -   i. This following procedure provides a method for selecting            the stepsize a_(l) in the modified Gauss-Newton iterative            formula            x _(l+1) =x ₁ +a ₁ Δx ₁  (273)        -    where Δx_(l) is the correction vector. Actually, because it            is not normalized, the correction Δx_(l) also contributes to            the size of the step. It is convenient to redefine            equation (273) as            x _(l+1) =x _(l) +a _(j) Δx _(l)  (274)        -    where a_(j) denotes the jth value of the step size at the            lth Gauss-Newton iteration.        -   ii. Once Δx_(l) is found from the Gauss-Newton equations,            the performance index PI_(l) is a function only of a_(j),            PI _(l)(a _(j))=PI _(l)(x _(l) +a _(j) Δx _(l))  (275)        -    and this is minimized by a judicious selection of a_(j).            Here, a_(j) is defined by the minimum of a quadratic            polynomial which passes through three data points (a_(j),            PI(a_(j)), j=1, 2, 3). For equally spaced values of a_(j),            the step size occurring at the minimum of this quadratic is            given by

$\begin{matrix}{a_{m} = \frac{{\left( {a_{2} + a_{3}} \right)P\;{I_{l}\left( a_{1} \right)}} - {2\left( {a_{1} + a_{3}} \right)P\;{I_{l}\left( a_{2} \right)}} + {\left( {a_{2} + a_{1}} \right)P\;{I_{l}\left( a_{3} \right)}}}{{2\; P\;{I_{l}\left( a_{1} \right)}} - {4\; P\;{I_{l}\left( a_{2} \right)}} + {2\; P\;{I_{l}\left( a_{3} \right)}}}} & (276)\end{matrix}$

-   -   -    where a₃>a₂≧0.        -   iii. The first of these data points is readily available,            namely, a₁=(0,PI_(l)(x₁)); and if            PI _(l)(1)<PI _(l)(0),  (277)        -    then a₂=1 gives the second data point and a₃=2a₂ gives the            third. However, if equation (302) is not satisfied, the            length of the interval is reduced by selecting a₂=½ and            a₃=2a₂=1, provided            PI _(l)(½)<PI _(l)(0)  (278)        -   iv. If this is successful, the next selection is a₂=¼ and            a₃=2a₂= 1/2, and subsequent selections are given by            repeatedly dividing a₂ by 2. This continues until            PI_(l)(a₂)<PI_(l)(a₁) or a threshold is crossed which causes            termination of the line search. After a_(m) is found, then            PI_(l)(a_(m)), PI_(l)(a₁) and PI_(l)(a₃) are compared to            determine which of these is the smallest. This is necessary            because the quadratic polynomial may not always provide a            good fit to the cost function and PI_(l)(a₂) or PI_(l)(a₃)            may be smaller than PI_(l)(a_(m)).

    -   o. Update the states using the selected stepsize:        -   i. Update the range states:            R1_(new) =R1_(old) +sG(1)  (279)            R2_(new) =R2_(old) +sG(2)  (280)        -    and insure R1 _(min)+0.1<R1 _(new)<R1 _(max)−0.1 and R2            _(min)+0.1<R2 _(new)<R2 _(max)−0.1.        -   ii. If frequency data is being processed update the            frequency state:            Fb _(new) =Fb _(old) +sG(3)  (281)        -    and insure 1<Fb_(new).

    -   p. Compute the new performance index (PI_(new)) based on the        updated states (R1 _(new),R2 _(new),Fb_(new)).

    -   q. Compute range, bearing, course and speed at tc:        -   i. Compute the x-component of target velocity (Vxt) and the            y-component of target velocity (Vyt):

$\begin{matrix}{{V\; x\; t} = \frac{{{R2}\;\sin\;{B2}} + {X\;{s2}} - {{R1}\;\sin\;{B1}} - {X\;{s1}}}{{t2} - {t1}}} & (282) \\{{V\; y\; t} = \frac{{{R2}\;\cos\;{B2}} + {Y\;{s2}} - {{R1}\;\cos\;{B1}} - {Y\;{s1}}}{{t2} - {t1}}} & (283)\end{matrix}$

-   -   -   ii. Compute target course (Ct) and target speed (Vt):

$\begin{matrix}{{C\; t} = {\tan^{- 1}\left( \frac{V\; x\; t}{V\; y\; t} \right)}} & (284)\end{matrix}$Vt=√{square root over (Vxt²+Vyt²)}  (285)

-   -   -   iii. Compute x-component of target position at tc (Xtc) and            y-component of target position at tc (Ytc):            Xtc=R2 sin B2+Xs2+Vxt(t2−tc)  (286)            Ytc=R2 cos B2+Ys2+Vyt(t2−tc)  (287)        -   iv. Compute x-component of range at tc with respect to own            ship (Rxoc) and y-component of range at tc with respect to            own ship (Ryoc):            Rxoc=Xtc−Xoc  (289)            Ryoc=Ytc−Yoc  (290)        -   v. Compute range at tc with respect to own ship (Roc) and            true bearing at tc with respect to own ship (Boc):            Roc=√{square root over (Rxoc²+Ryoc²)}  (291)

$\begin{matrix}{{Boc} = {\tan^{- 1}\left( \frac{Rxoc}{Ryoc} \right)}} & (292)\end{matrix}$

-   -   -   vi. Limit the range at tc with respect to own ship to the            maximum target range constraint.

Propagation path hypothesis testing can be performed by the endpoint MLEalgorithm on up to a maximum of four data segments which may be fromdifferent sonar arrays, and the endpoint MLE algorithm is capable ofprocessing an additional six azimuthal bearings only or azimuthalbearing/horizontal range segments (from any array) which may be directpath only. Each segment which contains either conical angle or frequencymeasurements is tested to determine whether the best propagation path isa direct path or is a bottom bounce single ray reversal path.Propagation path testing is performed by Alternating the propagationpath for each segment to be tested from a direct path to a bottom bouncepath, running the endpoint MLE algorithm for each propagation pathcombination and each appropriate port/starboard combination and bysaving the four best solution based on the performance index, along withthe associated port/starboard indicators at the time lines andpropagation paths for each segment. Thus, if there are four conicalangle only segments and six azimuthal bearing segments, then theendpoint MLE may be invoked up to sixty-four times if testing allpossible port/starboard combinations. If the selected time lines areassociated with conical angle measurement and bearing measurements areavailable close in time to the conical angle measurements which canremove all port/starboard ambiguity, then the endpoint MLE will tie downto the bearing measurements and port/starboard hypothesis testing willnot be performed.

Once the endpoint MLE has computed the four best solutions, the bestsolution is used to initialize the Cartesian coordinate MLE which willrefine the solution using the optimal propagation path combinations. TheCartesian coordinate MLE shall be allowed to change the port/starboarddesignations if a particular part/starboard combination has beenspecified.

Cartesian Coordinate MLE

1. Initialize the number of Gauss-Newton iterations to zero.

2. Determine the number of state variables as follows:

-   -   If at least three frequency measurements are available, then        frequency data will be processed, target base frequency shall be        estimated and the number of state variables (ns) shall be set to        five. Otherwise the number of state variables shall be four,        frequency data shall not be processed and target base frequency        shall not be estimated.        3. Initialize values for x-coordinate of target position at tm        (Xtm), y-component of target position at tm (Ytm), x-component        of target velocity (Vxt) and y-component of target velocity        (Vyt) using the outputs from the Endpoint MLE as follows:        Xtm=Roc sin Boc+Xoc−Vxt(tc−tm)  (293)        Ytm=Roc cos Boc+Yoc−Vyt(tc−tm)  (294)        Vxt=Vt sin Ct  (295)        Vyt=Vt cos Ct  (296)        where Roc is the range at tc with respect to own ship

Boc is the true bearing at tc with respect to own ship

Ct is the target course

Vt is the target speed

Xoc is the x-coordinate of own ship position at tc

Yoc is the y-coordinate of own ship position at tc

tm is the time of the most recent measurement

tc is current time

If frequency data are being processed, initialize the base frequency(Fb) to the base frequency output by the endpoint MLE.

4. Compute the Cartesian coordinate performance index (PI) based on theinitial states as follows:

-   -   a. First, for each measurement in the batch:        -   i. Compute the x-component of range at t_(i) with respect to            the sensor associated with the ith measurement (Rx_(i)) and            the y-component of range at t_(i) with respect to the sensor            associated with the ith measurement (Ry_(i)):            Rx _(i) =Xtm−Vxt(tm−t _(i))−Xo _(i)  (297)            Ry _(i) =Ytm−Vyt(tm−t _(i))−Yo _(i)  (298)        -   where Xo_(i) is x-coordinate of own ship position at t_(i)        -   Yo_(i) is y-coordinate of own ship position at t_(i)        -   t_(i) is the time of the ith measurement        -   tm is the time of the latest measurement        -   ii. Compute the range at t_(i) with respect to the sensor            associated with the ith measurement (R_(i));            R _(i) =√{square root over (Rx_(i) ²+Ry_(i) ²)}  (299)        -   iii. Compute the target image depth at t_(i) with respect to            the sensor associated with the ith measurement (Rz_(i)) and            D/E angle at t_(i) with respect to the sensor associated            with the ith measurement (θ_(i)).        -   iv. If the ith measurement is an azimuthal bearing:            -   1.) Compute the true bearing at t_(i) with respect to                the sensor associated with the ith measurement (B_(i)):

$\begin{matrix}{B_{i} = {\tan^{- 1}\left( \frac{{Rx}_{i}}{{Ry}_{i}} \right)}} & (300)\end{matrix}$

-   -   -   -   2.) Compute the bearing residual (RESb_(i)) such that                −π≦RESb_(i)≦π:                RESb _(i) =Bm _(i) −B _(i)  (301)            -    where Bm_(i) is the ith measured bearing            -   3.) Compute the normalized bearing residual ( RESb_(i)                ):

$\begin{matrix}{\overset{\_}{{RESb}_{i}} = \frac{{RESb}_{i}}{\sigma\; b_{i}}} & (302)\end{matrix}$

-   -   -    where σb_(i) is the measured bearing standard deviation        -   v. If the ith measurement is a conical angle:            -   1.) Compute the target image depth at t_(i) with respect                to the sensor associated with the ith measurement                (Rz_(i)) and D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)).            -   2.) If the D/E angle associated with the conical angle                measurement is valid:                -   a. Compute the true bearing at t_(i) with respect to                    the sensor associated with the ith measurement                    (B_(i)):

$\begin{matrix}{B_{i} = {\tan^{- 1}\left( \frac{{Rx}_{i}}{{Ry}_{i}} \right)}} & (303)\end{matrix}$

-   -   -   -   -   b. Compute the slant range at t_(i) with respect to                    the sensor associated with the ith measurement                    (Rs_(i)):                    Rs _(i) =√{square root over (Rx_(i) ²+Ry_(i)                    ²+Rz_(i) ²)}  (304)                -   c. Compute the conical angle at t_(i) with respect                    to the sensor associated with the ith measurement                    (β_(i)):

$\begin{matrix}{\beta_{i} = {\cos^{- 1}\left( \frac{\left( {{\cos\;{{Cs}_{i}\left( {{{Rx}_{i}\sin\;{Hs}_{i}} + {{Ry}_{i}\cos\;{Hs}_{i}}} \right)}} - {\sin\;{Cs}_{i}{Rz}_{i}}} \right)}{{Rs}_{i}} \right)}} & (305)\end{matrix}$

-   -   -   -   -   where Cs_(i) is the sensor cant angle at the ith                    measurement                -   Hs_(i) is the sensor heading at the ith measurement                -   d. Compute the conical angle (RESβ_(i)) such that                    −π≦RESβ_(i)≦π:                    RESβ _(i) =βm _(i)−β_(i)  (306)                -    where βm_(i) is the ith measured conical angle                -   e. Compute the normalized conical angle residual (                    RESβ_(i) ):

$\begin{matrix}{\overset{\_}{{RESb}_{i}} = \frac{{RESb}_{i}}{\sigma\; b_{i}}} & (307)\end{matrix}$

-   -   -   -   -    where σβ_(i) is the measured conical angle standard                    deviation.

        -   vi. If the ith measurement is a range:            -   1.) Compute the range residual (RESr_(i)):                RESr _(i) =Rm _(i) −R _(i)  (308)            -    where Rm_(i) is the ith measured range            -   2.) Compute the normalized range residual ( RESr_(i) ):

$\begin{matrix}{\overset{\_}{{RESr}_{i}} = \frac{{RESr}_{i}}{{\sigma r}_{i}}} & (309)\end{matrix}$

-   -   -   -    where σr_(i) is the measured range standard deviation

        -   vii. If frequency data are being processed and the ith            measurement is a frequency:            -   1.) Compute the x-component of target relative velocity                at t_(i) with respect to the sensor associated with the                ith measurement (Vx_(i)) and the y-component of target                relative velocity at t_(i) with respect to the sensor                associated with the ith measurement (Vy_(i)):                Vx _(i) =Vxt−Vxs _(i)  (310)                Vy _(i) =Vyt−Vys _(i)  (311)            -    where Vxs_(i) is the x-component of sensor velocity at                t_(i)                -   Vys_(i) is the y-component of sensor velocity at                    t_(i)

        -   2.) Compute the target image depth at t_(i) with respect to            the sensor associated with the ith measurement (Rz_(i)) and            D/E angle at t_(i) with respect to the sensor associated            with the ith measurement (θ_(i))

        -   3.) If the D/E angle associated with the frequency is valid,            compute the slant range at t_(i) with respect to the sensor            associated with the ith measurement (Rs_(i)):            Rs _(i) =√{square root over (R_(i) ²+Rz_(i) ²)}  (312)

        -   4.) Compute the estimated frequency at t_(i) with respect to            the sensor associated with the ith measurement:

$\begin{matrix}{f_{i} = {{Fb}\frac{{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}}{{cRs}_{i} + {VxtRx}_{i} + {VytRy}_{i}}}} & (313)\end{matrix}$

-   -   -   5.) Compute the frequency residual (RESf_(i))            RESf _(i) =fm _(i) −f _(i)  (314)        -    where fm_(i) is the ith measured frequency        -   6.) Compute the normalized frequency residual ( RESf_(i) ):

$\begin{matrix}{\overset{\_}{{RESf}_{i}} = \frac{{RESf}_{i}}{{\sigma f}_{i}}} & (315)\end{matrix}$

-   -   -    where σf_(i) is the measured frequency standard deviation.

    -   b. If a range constraint is being imposed, then the following        computations shall be performed:        -   i. Compute the range residual (RESr):            RESr=Rc−R  (316)        -    where Rc is the assumed target range        -   ii. Compute the normalized speed residual ( RESr):

$\begin{matrix}{\overset{\_}{RESr} = \frac{RESr}{\sigma\; R}} & (317)\end{matrix}$

-   -   -    where σR is the assumed target range standard deviation.

    -   c. If a speed constraint is being imposed, then the following        computations shall be performed.        -   i. Compute the estimated target speed:            V=√{square root over (Vxt²+Vyt²)}  (318)        -   ii. Compute the speed residual (RESv):            RESv=Vc−V  (319)        -    where Vc is the assumed target speed        -   iii. Compute the normalized speed residual ( RESv):

$\begin{matrix}{\overset{\_}{RESv} = \frac{RESv}{\sigma V}} & (320)\end{matrix}$

-   -   -    where σV is the assumed target speed standard deviation.

    -   d. Compute the performance index as one half of the sum of the        squared normalized residuals.        5. Gauss-Newton iterations shall be performed as described in        paragraphs a through n below, until the algorithm converges as        described in paragraph n or until twenty-five iterations have        been performed.

    -   a. For each measurement in the batch:        -   i. Compute the x-component of range at t_(i) with respect to            the sensor associated with the ith measurement (Rx_(i)) and            the y-component of range at t_(i) with respect to the sensor            associated with the ith measurement (Ry_(i)):            Rx _(i) =Xtm−Vxt(tm−t _(i))−Xo _(i)  (321)            Ry _(i) =Ytm−Vyt(tm−t _(i))−Yo _(i)  (322)        -   where Xo_(i) is x-coordinate of own ship position at t_(i)        -   Yo_(i) is y-coordinate of own ship position at t_(i)        -   t_(i) is the time of the ith measurement        -   ii. Compute the range at t_(i) with respect to the sensor            associated with the ith measurement (R_(i)):

$\begin{matrix}{R_{i} = \sqrt{{Rx}_{i}^{2} + {Ry}_{i}^{2}}} & (323)\end{matrix}$

-   -   -   iii. Compute the target image depth at t_(i) with respect to            the sensor associated with the ith measurement (Rz_(i)) and            D/E angle at t_(i) with respect to the sensor associated            with the ith measurement (θ_(I))        -   iv. If the ith measurement is an azimuthal bearing:            -   1.) Compute the partial derivative of true bearing at                t_(i) with respect to the sensor associated with the ith                measurement with respect to the x-coordinate of target                position at

${t_{m}\left( \frac{\partial B_{i}}{\partial{Xtm}} \right)},$

-   -   -   -    the partial derivative of true bearing at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-coordinate of target                position at t_(m)

$\left( \frac{\partial B_{i}}{\partial{Ytm}} \right),$

-   -   -   -    the partial derivative of true bearing at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the x-component of target                velocity

$\left( \frac{\partial B_{i}}{\partial{Vxt}} \right),$

-   -   -   -    and the partial derivative of true bearing at t_(i)                with respect to the sensor associated with the ith                measurement with respect to the y-component of target                velocity

$\begin{matrix}{\left( \frac{\partial B_{i}}{\partial{Yxt}} \right)\text{:}} & \; \\{\frac{\partial B_{i}}{\partial{Xtm}} = \frac{{Ry}_{i}}{R_{i}^{2}}} & (324) \\{\frac{\partial B_{i}}{\partial{Ytm}} = {- \frac{{Rx}_{i}}{R_{i}^{2}}}} & (325) \\{\frac{\partial B_{i}}{\partial{Vxt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial B_{i}}{\partial{Xtm}}}} & (326) \\{\frac{\partial B_{i}}{\partial{Vyt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial B_{i}}{\partial{Ytm}}}} & (327)\end{matrix}$

-   -   -   -   2.) Compute the bearing residual (RESb_(i)) such that                −π≦RESb_(i)≦π:                RESb _(i) =Bm _(i) −B _(i)  (328)            -    where Bm_(i) is the ith measured bearing            -   3.) Compute the normalized bearing residual ( RESb_(i) )                and normalized partial derivative

$\begin{matrix}{\left( {\frac{\overset{\_}{\partial B_{i}}}{\partial{Xtm}},\frac{\overset{\_}{\partial B_{i}}}{\partial{Ytm}},\frac{\overset{\_}{\partial B_{i}}}{\partial{Vxt}},\frac{\overset{\_}{\partial B_{i}}}{\partial{Vyt}}} \right)\text{:}} & \; \\{\overset{\_}{{RESb}_{i}} = \frac{{RESb}_{i}}{{\sigma b}_{i}}} & (329) \\{\frac{\overset{\_}{\partial B_{i}}}{\partial{Xtm}} = \frac{\frac{\partial B_{i}}{\partial{Xtm}}}{{\sigma b}_{i}}} & (330) \\{\frac{\overset{\_}{\partial B_{i}}}{\partial{Ytm}} = \frac{\frac{\partial B_{i}}{\partial{Ytm}}}{{\sigma b}_{i}}} & (331) \\{\frac{\overset{\_}{\partial B_{i}}}{\partial{Vxt}} = \frac{\frac{\partial B_{i}}{\partial{Vxt}}}{{\sigma b}_{i}}} & (332) \\{\frac{\overset{\_}{\partial B_{i}}}{\partial{Vyt}} = \frac{\frac{\partial B_{i}}{\partial{Vyt}}}{{\sigma b}_{i}}} & (333)\end{matrix}$

-   -   -   -    where σb_(i) is the measured bearing standard deviation            -   4.) If frequency data are not being processed, then set                the next row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial B_{i}}}{\partial{Xtm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Ytm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vxt}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vyt}}\overset{\_}{{RESB}_{i}}} \right\rbrack & (334)\end{matrix}$

-   -   -   -   If frequency data are being processed, then set the next                row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial B_{i}}}{\partial{Xtm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Ytm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vxt}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vyt}}0\mspace{14mu}\overset{\_}{{RESB}_{i}}} \right\rbrack & (335)\end{matrix}$

-   -   -   v. If the ith measurement is a conical angle:            -   1.) Compute the target image depth at t_(i) with respect                to the sensor associated with the ith measurement                (Rz_(i)) and D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)).            -   2.) If the D/E angle associated with the conical angle                measurement is valid:                -   a Compute the true bearing at t_(i) with respect to                    the sensor associated with the ith measurement                    (B_(i)):

$\begin{matrix}{B_{i} = {\tan^{- 1}\left( \frac{{Rx}_{i}}{{Ry}_{i}} \right)}} & (336)\end{matrix}$

-   -   -   -   -   b Compute the slant range at t_(i) with respect to                    the sensor associated with the ith measurement                    (Rs_(i)):                    Rs _(i) =√{square root over (Rx_(i) ²+Ry_(i)                    ²+Rz_(i) ²)}  (337)                -   c Compute the conical angle at t_(i) with respect to                    the sensor associated with the ith measurement                    (β_(i)):

$\begin{matrix}{\beta_{i} = {\cos^{- 2}\left( \frac{\left( {{\cos\;{{Cs}_{i}\left( {{{Rx}_{i}\sin\;{Hs}_{i}} + {{Ry}_{i}\cos\;{Hs}_{i}}} \right)}} - {\sin\;{Cs}_{i}{Rz}_{i}}} \right)}{{Rs}_{i}} \right)}} & (338)\end{matrix}$

-   -   -   -   -    where Cs_(i) is the sensor cant angle at the ith                    measurement and Hs_(i) is the sensor heading at the                    ith measurement                -   d Compute the partial derivative of conical angle at                    t_(i) with respect to the sensor associated with the                    ith measurement with respect to the x-coordinate of                    target position at t_(m)

$\left( \frac{\partial\beta_{i}}{\partial{Xtm}} \right),$

-   -   -   -   -    the partial derivative of conical angle at t_(i)                    with respect to the sensor associated with the ith                    measurement with respect to the y-coordinate of                    target position at tm

$\left( \frac{\partial\beta_{i}}{\partial{Ytm}} \right),$

-   -   -   -   -    the partial derivative of conical angle at t_(i)                    with respect to the sensor associated with the ith                    measurement with respect to the x-component of                    target velocity

$\left( \frac{\partial\beta_{i}}{\partial{Vxt}} \right),$

-   -   -   -   -    and the partial derivative of conical angle at                    t_(i) with respect to the sensor associated with the                    ith measurement with respect to the y-component of                    target velocity

$\left( \frac{\partial\beta_{i}}{\partial{Vyt}} \right)\text{:}$$\begin{matrix}{\frac{\partial\beta_{i}}{\partial{Xtm}} = {- \frac{\begin{matrix}\left( {\cos\;{{Cs}_{i}\left( {{\left( {{Ry}_{i}^{2} + {Rz}_{i}^{2}} \right)\sin\;{Hs}_{i}} -} \right.}} \right. \\\left. {\left. {{Rx}_{i}{Ry}_{i}\cos\;{Hs}_{i}} \right) + {{Rx}_{i}{Rz}_{i}\sin\;{Cs}_{i}}} \right)\end{matrix}}{\sin\;\beta_{i}R_{i}^{3}}}} & (339) \\{\frac{\partial\beta_{i}}{\partial{Ytm}} = {- \frac{\begin{matrix}\left( {\cos\;{{Cs}_{i}\left( {{\left( {{Rx}_{i}^{2} + {Rz}_{i}^{2}} \right)\cos\;{Hs}_{i}} -} \right.}} \right. \\\left. {\left. {{Rx}_{i}{Ry}_{i}\sin\;{Hs}_{i}} \right) + {{Ry}_{i}{Rz}_{i}\sin\;{Cs}_{i}}} \right)\end{matrix}}{\sin\;\beta_{i}R_{i}^{3}}}} & (340) \\{\frac{\partial\beta_{i}}{\partial{Vxt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial\beta_{i}}{\partial{Xtm}}}} & (341) \\{\frac{\partial\beta_{i}}{\partial{Vyt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial\beta_{i}}{\partial{Ytm}}}} & (342)\end{matrix}$

-   -   -   -   -   e Compute the conical angle (RESβ_(i)) such that                    −π≦RESβ_(i)≦π:                    RESβ _(i) =βm _(i)−β_(i)  (343)                -    where βm_(i) is the ith measured conical angle.                -   f Compute the normalized conical angle residual (                    RESβ_(i) ) and normalized partial derivatives

$\left( {\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Xtm}},\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Ytm}},\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Vxt}},\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Vyt}}} \right)\text{:}$$\begin{matrix}{\overset{\_}{{RES\beta}_{i}} = \frac{{RES\beta}_{i}}{{\sigma\beta}_{i}}} & (344) \\{\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Xtm}} = \frac{\frac{\partial\beta_{i}}{\partial{Xtm}}}{{\sigma\beta}_{i}}} & (345) \\{\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Ytm}} = \frac{\frac{\partial\beta_{i}}{\partial{Ytm}}}{{\sigma\beta}_{i}}} & (346) \\{\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Vxt}} = \frac{\frac{\partial\beta_{i}}{\partial{Vxt}}}{{\sigma\beta}_{i}}} & (347) \\{\frac{\overset{\_}{\partial\beta_{i}}}{\partial{Vyt}} = \frac{\frac{\partial\beta_{i}}{\partial{Vyt}}}{{\sigma\beta}_{i}}} & (348)\end{matrix}$

-   -   -   -   -    where σβ_(I) is the measured conical angle standard                    deviation                -   g If frequency data are not being processed, set the                    next row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial B_{i}}}{\partial{Xtm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Ytm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vxt}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vyt}}\overset{\_}{{RES\beta}_{i}}} \right\rbrack & (349)\end{matrix}$

-   -   -   -   -   If frequency data are being processed, then set the                    next row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial B_{i}}}{\partial{Xtm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Ytm}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vxt}}\frac{\overset{\_}{\partial B_{i}}}{\partial{Vyt}}0\mspace{14mu}\overset{\_}{{RES\beta}_{i}}} \right\rbrack & (350)\end{matrix}$

-   -   -   vi. If the ith measurement is a range:            -   1.) Compute the partial derivative of range at t_(i)                with respect to the sensor associated with the ith                measurement with respect to the x-coordinate of target                position at t_(m)

$\left( \frac{\partial R_{i}}{\partial{Xtm}} \right),$

-   -   -   -    the partial derivative of range at t_(i) with respect                to the sensor associated with the ith measurement with                respect to the y-coordinate of target position at t_(m)

$\left( \frac{\partial R_{i}}{\partial{Ytm}} \right),$

-   -   -   -    the partial derivative of range at t_(i) with respect                to the sensor associated with the ith measurement with                respect to the x-component of target velocity

$\left( \frac{\partial R_{i}}{\partial{Vxt}} \right),$

-   -   -   -    and the partial derivative of range at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-component of target                velocity

$\left( \frac{\partial R_{i}}{\partial{Vyt}} \right)\text{:}$$\begin{matrix}{\frac{\partial R_{i}}{\partial{Xtm}} = \frac{{Rx}_{i}}{R_{i}}} & (351) \\{\frac{\partial R_{i}}{\partial{Ytm}} = \frac{{Ry}_{i}}{R_{i}}} & (352) \\{\frac{\partial R_{i}}{\partial{Vxt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial R_{i}}{\partial{Xtm}}}} & (353) \\{\frac{\partial R_{i}}{\partial{Vyt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial R_{i}}{\partial{Ytm}}}} & (354)\end{matrix}$

-   -   -   -   2.) Compute the range residual (RESr_(i)):                RESr _(i) =Rm _(i) −R _(i)  (355)            -    where Rm_(i) is the ith measured range            -   3.) Compute the normalized range residual ( RESr_(i) )                and normalized partial derivative

$\left( {\frac{\overset{\_}{\partial R_{i}}}{\partial{Xtm}},\frac{\overset{\_}{\partial R_{i}}}{\partial{Ytm}},\frac{\overset{\_}{\partial R_{i}}}{\partial{Vxt}},\frac{\overset{\_}{\partial R_{i}}}{\partial{Vyt}}} \right)\text{:}$$\begin{matrix}{\overset{\_}{{RESr}_{i}} = \frac{{RESr}_{i}}{{\sigma r}_{i}}} & (356) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Xtm}} = \frac{\frac{\partial R_{i}}{\partial{Xtm}}}{{\sigma r}_{i}}} & (357) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Ytm}} = \frac{\frac{\partial R_{i}}{\partial{Ytm}}}{{\sigma r}_{i}}} & (358) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Vxt}} = \frac{\frac{\partial R_{i}}{\partial{Vxt}}}{{\sigma r}_{i}}} & (359) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Vyt}} = \frac{\frac{\partial R_{i}}{\partial{Vyt}}}{{\sigma r}_{i}}} & (360)\end{matrix}$

-   -   -   -    where σr_(i) is the measured range standard deviation.            -   4.) If frequency data are not being processed, then set                the next row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\overset{\_}{\frac{\partial R_{i}}{\partial{Xtm}}}\mspace{14mu}\overset{\_}{\frac{\partial R_{i}}{\partial{Ytm}}}\mspace{14mu}\overset{\_}{\frac{\partial R_{i}}{\partial{Vxt}}}\mspace{14mu}\overset{\_}{\frac{\partial R_{i}}{\partial{Vyt}}}\mspace{14mu}\overset{\_}{{RESr}_{i}}} \right\rbrack & (361)\end{matrix}$

-   -   -   -   If frequency data are being processed, then set the next                row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\overset{\_}{\frac{\partial R_{i}}{\partial{Xtm}}}\mspace{14mu}\overset{\_}{\frac{\partial R_{i}}{\partial{Ytm}}}\mspace{14mu}\overset{\_}{\frac{\partial R_{i}}{\partial{Vxt}}}\mspace{14mu}\overset{\_}{\frac{\partial R_{i}}{\partial{Vyt}}}\mspace{20mu} 0\mspace{20mu}\overset{\_}{{RESr}_{i}}} \right\rbrack & (362)\end{matrix}$

-   -   -   vii. If frequency data are being processed and the ith            measurement is a frequency:            -   1.) Compute the x-component of target relative velocity                at t_(i) with respect to the sensor associated with the                ith measurement (Vx_(i)) and the y-component of target                relative velocity at t_(i) with respect to the sensor                associated with the ith measurement (Vy_(i)):                Vx _(i) =Vxt−Vxs _(i)  (363)                Vy _(i) =Vyt−Vys _(i)  (364)            -    where Vxs_(i) is the x-component of sensor velocity at                t_(i) and Vys_(i) is the y-component of sensor velocity                at t_(i).            -   2.) Compute the target image depth at t_(i) with respect                to the sensor associated with the ith measurement                (Rz_(i)) and D/E angle at t_(i) with respect to the                sensor associated with the ith measurement (θ_(i)).            -   3.) If the D/E angle associated with the frequency is                valid, compute the slant range at t_(i) with respect to                the sensor associated with the ith measurement (Rs_(i)):                Rs _(i) √{square root over (R_(i) ²+Rz_(i) ²)}  (365)            -   4.) Compute the partial derivative of frequency at t_(i)                with respect to the sensor associated with the ith                measurement with respect to the x-coordinate of target                position at t_(m)

$\left( \frac{\partial f_{i}}{\partial{Xtm}} \right),$

-   -   -   -    the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-coordinate of target                position at t_(m)

$\left( \frac{\partial f_{i}}{\partial{Ytm}} \right),$

-   -   -   -    the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the x-component of target                velocity

$\left( \frac{\partial f_{i}}{\partial{Vxt}} \right),$

-   -   -   -    the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to the y-component of target                velocity

$\left( \frac{\partial f_{i}}{\partial{Vyt}} \right)$

-   -   -   -    and the partial derivative of frequency at t_(i) with                respect to the sensor associated with the ith                measurement with respect to base frequency

$\begin{matrix}{{\left( \frac{\partial f_{i}}{\partial{Fb}} \right):}\mspace{635mu}} & \; \\{{{\frac{\partial f_{i}}{\partial{Xtm}} = \frac{Fb}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)^{2}}}\quad}{\quad{\bullet{\quad\quad}\left( {{\left( {{{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)\left( {{\frac{c}{{Rs}_{i}}{Rx}_{i}} + {Vxs}_{i}} \right)} - {\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}} \right){Vx}_{i}}} \right)}}} & (366) \\{{{\frac{\partial f_{i}}{\partial{Ytm}} = \frac{Fb}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)^{2}}}\quad}{\quad{\bullet{\quad\quad}\left( {{\left( {{{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)\left( {{\frac{c}{{Rs}_{i}}{Ry}_{i}} + {Vys}_{i}} \right)} - {\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}} \right){Vy}_{i}}} \right)}}} & (367) \\{\frac{\partial f_{i}}{\partial{Vxt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial f_{i}}{\partial{Xtm}}}} & (368) \\{\frac{\partial f_{i}}{\partial{Vyt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial f_{i}}{\partial{Ytm}}}} & (369) \\{\frac{\partial f_{i}}{\partial{Fb}} = \frac{{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}}{\left( {{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}} + {{Rx}_{i}{Vx}_{i}} + {{Ry}_{i}{Vy}_{i}}} \right)}} & (370)\end{matrix}$

-   -   -   -    where c is the average speed of sound            -   5.) Compute the estimated frequency at t_(i) with                respect to the sensor associated with the ith                measurement:

$\begin{matrix}{f_{i} = {{Fb}\frac{{cRs}_{i} + {{Vxs}_{i}{Rx}_{i}} + {{Vys}_{i}{Ry}_{i}}}{{cRs}_{i} + {VxtRx}_{i} + {VytRy}_{i}}}} & (371)\end{matrix}$

-   -   -   -   6.) Compute the frequency residual (RESf_(i)):                RESf _(i) =fm _(i) −f _(i)  (372)            -    where fm_(i) is the ith measured frequency            -   7.) Compute the normalized frequency residual ( RESf_(i)                ) and normalized partial derivatives

$\begin{matrix}{\left\lbrack {\overset{\_}{\frac{\partial f_{i}}{\partial{Xtm}}},\overset{\_}{\frac{\partial f_{i}}{\partial{Ytm}}},\;\overset{\_}{\frac{\partial f_{i}}{\partial{Vxt}}},\;\overset{\_}{\frac{\partial f_{i}}{\partial{Vyt}}},\;\overset{\_}{\frac{\partial f_{i}}{\partial{Fb}}}} \right\rbrack:} & \; \\{\overset{\_}{\frac{\partial f_{i}}{\partial{Xtm}}} = \frac{\frac{\partial f_{i}}{\partial{Xtm}}}{\sigma\; f_{i}}} & (374) \\{\overset{\_}{\frac{\partial f_{i}}{\partial{Ytm}}} = \frac{\frac{\partial f_{i}}{\partial{Ytm}}}{\sigma\; f_{i}}} & (375) \\{\overset{\_}{\frac{\partial f_{i}}{\partial{Vxt}}} = \frac{\frac{\partial f_{i}}{\partial{Vxt}}}{\sigma\; f_{i}}} & (376) \\{\overset{\_}{\frac{\partial f_{i}}{\partial{Vyt}}} = \frac{\frac{\partial f_{i}}{\partial{Vyt}}}{\sigma\; f_{i}}} & (377) \\{\overset{\_}{\frac{\partial f_{i}}{\partial{Fb}}} = \frac{\frac{\partial f_{i}}{\partial{Fb}}}{\sigma\; f_{i}}} & (378)\end{matrix}$

-   -   -   -    where σf_(i) is the measured frequency standard                deviation.            -   8.) Set the next row of the augmented Jacobian matrix H                to:

$\begin{matrix}\left\lbrack {\overset{\_}{\frac{\partial f_{i}}{\partial{Xtm}}}\mspace{14mu}\overset{\_}{\frac{\partial f_{i}}{\partial{Ytm}}}\mspace{14mu}\overset{\_}{\frac{\partial f_{i}}{\partial{Vxt}}}\mspace{14mu}\overset{\_}{\frac{\partial f_{i}}{\partial{Vyt}}}\mspace{14mu}\overset{\_}{\frac{\partial f_{i}}{\partial{Fb}}}\;\overset{\_}{{RESf}_{i}}} \right\rbrack & (379)\end{matrix}$

-   -   b. If a range constraint is being imposed, the following        computations shall be performed:        -   i. Compute the partial derivative of range at t_(i) with            respect to the sensor associated with the ith measurement            with respect to the x-coordinate of target position at t_(m)

$\left( \frac{\partial R_{i}}{\partial{Xtm}} \right),$

-   -   -    the partial derivative of range at t_(i) with respect to            the sensor associated with the ith measurement with respect            to the y-coordinate of target position at t_(m)

$\left( \frac{\partial R_{i}}{\partial{Ytm}} \right),$

-   -   -    the partial derivative of range at t_(i) with respect to            the sensor associated with the ith measurement with respect            to the x-component of target velocity

$\left( \frac{\partial R_{i}}{\partial{Vxt}} \right),$

-   -   -    and the partial derivative of range at t_(i) with respect            to the sensor associated with the ith measurement with            respect to the y-component of target velocity

$\begin{matrix}{\left( \frac{\partial R_{i}}{\partial{Vyt}} \right):} & \; \\{\frac{\partial R_{i}}{\partial{Xtm}} = \frac{{Rx}_{i}}{R_{i}}} & (380) \\{\frac{\partial R_{i}}{\partial{Ytm}} = \frac{{Ry}_{i}}{R_{i}}} & (381) \\{\frac{\partial R_{i}}{\partial{Vxt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial R_{i}}{\partial{Xtm}}}} & (382) \\{\frac{\partial R_{i}}{\partial{Vyt}} = {{- \left( {t_{m} - t_{i}} \right)}\frac{\partial R_{i}}{\partial{Ytm}}}} & (383)\end{matrix}$

-   -   -   ii. Compute the range residual (RESr_(i)):            RESr _(i) =Rc−R  (384)        -    where Rc is the assumed target range        -   iii Compute the normalized range residual ( RESr_(i) ) and            normalized partial derivatives

$\begin{matrix}{\left\lbrack {\overset{\_}{\frac{\partial R_{i}}{\partial{Xtm}}}\mspace{11mu},\overset{\_}{\frac{\partial R_{i}}{\partial{Ytm}}},\;\overset{\_}{\frac{\partial R_{i}}{\partial{Vxt}}},\;\overset{\_}{\frac{\partial R_{i}}{\partial{Vyt}}}} \right\rbrack:} & \; \\{\overset{\_}{{RESr}_{i}} = \frac{{RESr}_{i}}{\sigma\; R}} & (385) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Xtm}} = \frac{\frac{\partial R_{i}}{\partial{Xtm}}}{\sigma\; R}} & (386) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Ytm}} = \frac{\frac{\partial R_{i}}{\partial{Ytm}}}{\sigma\; R}} & (387) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Vxt}} = \frac{\frac{\partial R_{i}}{\partial{Vxt}}}{\sigma\; R}} & (388) \\{\frac{\overset{\_}{\partial R_{i}}}{\partial{Vyt}} = \frac{\frac{\partial R_{i}}{\partial{Vyt}}}{\sigma\; R}} & (389)\end{matrix}$

-   -   -    where σR is the measured range standard deviation.        -   iv. If frequency data are not being processed, then set the            next row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial R_{i}}}{\partial{Xtm}}\mspace{14mu}\frac{\overset{\_}{\partial R_{i}}}{\partial{Ytm}}\mspace{11mu}\frac{\overset{\_}{\partial R_{i}}}{\partial{Vxt}}\mspace{14mu}\frac{\overset{\_}{\partial R_{i}}}{\partial{Vyt}}\overset{\_}{{RESr}_{i}}} \right\rbrack & (390)\end{matrix}$

-   -   -   If frequency data are being processed, then set the next row            of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial R_{i}}}{\partial{Xtm}}\mspace{14mu}\frac{\overset{\_}{\partial R_{i}}}{\partial{Ytm}}\mspace{11mu}\frac{\overset{\_}{\partial R_{i}}}{\partial{Vxt}}\mspace{14mu}\frac{\overset{\_}{\partial R_{i}}}{\partial{Vyt}}\mspace{20mu} 0\mspace{14mu}\overset{\_}{{RESr}_{i}}} \right\rbrack & (391)\end{matrix}$

-   -   c. If a speed constraint is being imposed, the following        computations shall be performed:        -   i. Compute the estimated target speed:            V+√{square root over (Vxt²+Vyt²)}  (392)        -   ii. Compute the partial derivative of target speed with            respect to the x-coordinate of target position at t_(m)

$\left( \frac{\partial V}{\partial{Xtm}} \right),$

-   -   -    the partial derivative of target speed with respect to the            y-coordinate of target position at t_(m)

$\left( \frac{\partial V}{\partial{Ytm}} \right),$

-   -   -    the partial derivative of target speed with respect to the            x-component of target velocity

$\left( \frac{\partial V}{\partial{Vxt}} \right)$

-   -   -    and the partial derivative of target speed with respect to            the y-component of target velocity

$\begin{matrix}{\left( \frac{\partial V}{\partial{Vyt}} \right):} & \; \\{\frac{\partial V}{\partial{Xtm}} = 0} & (393) \\{\frac{\partial V}{\partial{Ytm}} = 0} & (394) \\{\frac{\partial V}{\partial{Vxt}} = \frac{Vxt}{V}} & (395) \\{\frac{\partial V}{\partial{Vyt}} = \frac{Vyt}{V}} & (396)\end{matrix}$

-   -   -   iii. Compute the speed residual (RESv):            RES _(v) =Vc−V  (397)        -    where Vc is the assumed target speed        -   iv. Compute the normalized speed residual ( RESv) and            normalized partial derivatives

$\begin{matrix}{\left\lbrack {\frac{\overset{\_}{\partial V}}{\partial{Xtm}},\mspace{14mu}\frac{\overset{\_}{\partial V}}{\partial{Ytm}},\mspace{11mu}\frac{\overset{\_}{\partial V}}{\partial{Vxt}},\mspace{14mu}\frac{\overset{\_}{\partial V}}{\partial{Vyt}}} \right\rbrack:} & \; \\{\overset{\_}{RESv} = \frac{RESv}{\sigma\; R}} & (398) \\{\frac{\overset{\_}{\partial V}}{\partial{Xtm}} = \frac{\frac{\partial V}{\partial{Xtm}}}{\sigma\; V}} & (399) \\{\frac{\overset{\_}{\partial V}}{\partial{Ytm}} = \frac{\frac{\partial V}{\partial{Ytm}}}{\sigma\; V}} & (400) \\{\frac{\overset{\_}{\partial V}}{\partial{Vxt}} = \frac{\frac{\partial V}{\partial{Vxt}}}{\sigma\; V}} & (401) \\{\frac{\overset{\_}{\partial V}}{\partial{Vyt}} = \frac{\frac{\partial V}{\partial{Vyt}}}{\sigma\; V}} & (402)\end{matrix}$

-   -   -    where σV is the assumed target speed standard deviation.        -   v. If frequency data are not being processed, then set the            next row of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial V}}{\partial{Xtm}}\mspace{14mu}\frac{\overset{\_}{\partial V}}{\partial{Ytm}}\mspace{11mu}\frac{\overset{\_}{\partial V}}{\partial{Vxt}}\mspace{14mu}\frac{\overset{\_}{\partial V}}{\partial{Vyt}}\mspace{14mu}\overset{\_}{RESv}} \right\rbrack & (403)\end{matrix}$

-   -   -   If frequency data are being processed, then set the next row            of the augmented Jacobian matrix H to:

$\begin{matrix}\left\lbrack {\frac{\overset{\_}{\partial V}}{\partial{Xtm}}\mspace{14mu}\frac{\overset{\_}{\partial V}}{\partial{Ytm}}\mspace{11mu}\frac{\overset{\_}{\partial V}}{\partial{Vxt}}\mspace{14mu}\frac{\overset{\_}{\partial V}}{\partial{Vyt}}\mspace{20mu} 0\mspace{14mu}\overset{\_}{RESv}} \right\rbrack & (404)\end{matrix}$

-   -   d. Reorder the rows of the matrix H such that a zero valued        partial derivative does not appear along the diagonal.    -   e. Perform the Householder transformation on the m×n+1 matrix H.    -   f. Extract the upper triangular matrix R from the upper left        hand corner of the transformed matrix H.    -   g. Compute R⁻¹ by back-substitution.    -   h. Extract the Y vector from the upper right hand corner of the        transformed matrix H.    -   i. Compute the gain vector (G):        G=R ⁻¹ Y  (405)    -   j. Determine if the gain is near zero. If both |G(1)| and |G(2)|        are less than 0.1 and |G(3)| and |G(4)| are less than 0.01, then        the algorithm has converged, Gauss Newton iterations shall        terminate, and processing shall be performed as described in        paragraph 6. Otherwise, processing shall continue as described        below.    -   k. Compute the stepsize as described in n. above.    -   l. Update the states using the optimal stepsize(s):        -   i. Update the position and velocity states:            Xtm=Xtm+sG(1)  (406)            Ytm=Ytm+sG(2)  (407)            Vxt=Vxt+sG(3)  (408)            Vyt=Vyt+sG(4)  (409).        -   ii. If frequency data are being processed, update the            frequency state:            Fb=Fb+sG(5)  (410)        -   iii Compute range with respect to own ship at t_(m) (Rom)            and target speed (Vt) as            Rom=√{square root over ((Xtm−Xom)²+(Ytm−Yom)²)}{square root            over ((Xtm−Xom)²+(Ytm−Yom)²)}  (411)            Vt=√{square root over (Vxt²+Vyt²)}  (412)        -   iv. Insure R_(min)+0.1<Rom<R_(max) and            V_(min)+0.1<Vt<V_(max). If either Rm or Vt is out of bounds,            limit the appropriate parameter and recompute Xtm, Ytm, Vxt            and Vyt.    -   m. Compute the new performance index (PI_(new)) based on the        updated states (Xtm, Ytm, Vxt, Vyt, Fb)    -   n. Compute range, bearing, course and speed at current time:        -   i. Compute target course (Ct) and target speed (Vt):

$\begin{matrix}{{Ct} = {\tan^{- 1}\left( \frac{Vxt}{Vyt} \right)}} & (413)\end{matrix}$Vt=√{square root over (Vxt²+Vyt²)}  (414)

-   -   -   ii. Compute x-coordinate of target position at tc(Xtc) and            y-coordinate of target position at tc(Ytc):            Xtc=Xtm+Vxt(tc−tm)  (415)            Ytc=Ytm+Vyt(tc−tm)  (416)        -   iii. Compute x-component of range at tc(Rxc) and y-component            of range at tc with respect to own ship (Ryc):            Rxc=Xtc−Xoc  (417)            Ryc=Ytc−Yoc  (418)        -    where Xoc is the x-coordinate of own ship position at tc            and Yoc is the y-coordinate of own ship position at tc.        -   iv. Compute range at tc with respect to own ship (Rc) and            true bearing at tc with respect to own ship (Bc):            Rc=√{square root over (Rxc²+Ryc²)}  (419)

$\begin{matrix}{{Bc} = {\tan^{- 1}\left( \frac{Rxc}{Ryc} \right)}} & (420)\end{matrix}$

-   -   -   v. Limit range at tc with respect to own ship to a maximum            of the target maximum range.        -   vi. Limit target speed to a maximum of the target maximum            speed.

    -   o. Determine if the change in the performance index is        negligible. If so, processing shall terminate, otherwise,        Gauss-Newton iterations shall continue.        -   i. Compute change in the performance index (ΔPI):

$\begin{matrix}{{{{\left. 1 \right).\mspace{14mu}{If}}\mspace{14mu}{PI}_{old}} > 0},} & \; \\{\mspace{40mu}{{\Delta\;{PI}} = \frac{{{PI}_{new} - {PI}_{old}}}{{PI}_{old}}}} & (421)\end{matrix}$

-   -   -   -   2.) If PI_(old)=0,                ΔPI=0  (422)

        -   ii. If ΔPI≦0.00001 and PI_(new)≦threshold_(cc), stop            iterating.            6. Compute the ns by ns Cartesian coordinate covariance            matrix:            P=R ⁻¹ R ^(−T)  (423)            7. Extrapolate the covariance matrix forward to current            time:

    -   a. If frequency data are not being processed, the transition        matrix Φ shall be defined as follows:

$\begin{matrix}{\phi = \begin{bmatrix}1 & 0 & {{tc} - {tm}} & 0 \\0 & 1 & 0 & {{tc} - {tm}} \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{bmatrix}} & (424)\end{matrix}$

-   -   b. If frequency data are being processed, the transition matrix        Φ shall be defined as follows:

$\begin{matrix}{\phi = \begin{bmatrix}1 & 0 & {{tc} - {tm}} & 0 & 0 \\0 & 1 & 0 & {{tc} - {tm}} & 0 \\0 & 0 & 1 & 0 & 0 \\0 & 0 & 0 & 1 & 0 \\0 & 0 & 0 & 0 & 1\end{bmatrix}} & (425)\end{matrix}$

-   -   c. The covariance matrix at tc shall be extrapolated as follows:        p=φPφ ^(T)  (426)        8. Compute target range, bearing, course, speed and base        frequency standard deviations:

$\begin{matrix}{\sigma_{R} = \sqrt{\frac{{P_{11}{Rxc}^{2}} + {2P_{12}{RxcRyc}} + {P_{22}{Ryc}^{2}}}{{Rc}^{2}}}} & (427) \\{\sigma_{B} = \sqrt{\frac{{P_{11}{Ryc}^{2}} - {2P_{12}{RxcRyc}} + {P_{22}{Rxc}^{2}}}{{Rc}^{4}}}} & (428) \\{\sigma_{C} = \sqrt{\frac{{P_{33}{Vyt}^{2}} - {2P_{34}{Vxt}*{Vyt}} + {P_{44}{Vyt}^{2}}}{{Vt}^{4}}}} & (429) \\{\sigma_{S} = \sqrt{\frac{{P_{33}{Vxt}^{2}} + {2P_{34}{Vxt}*{Vyt}} + {P_{44}{Vyt}^{2}}}{{Vt}^{2}}}} & (430)\end{matrix}$If frequency data are being processed.σ_(F) =√{square root over (P₅₅)}  (431)9. Compute major and minor localization ellipse axis length (X_(maj),X_(min)) and orientation of major axis from North (ORIEN):

$\begin{matrix}{\lambda_{maj} = \frac{P_{11} + P_{22} + \sqrt{\left( {P_{11} - P_{22}} \right)^{2} + {4P_{12}^{2}}}}{2}} & (432) \\{\lambda_{m\; i\; n} = \frac{P_{11} + P_{12} - \sqrt{\left( {P_{11} - P_{22}} \right)^{2} + {4P_{12}^{5}}}}{2}} & (433) \\{x_{maj} = {2.1459\sqrt{\lambda_{maj}}}} & (434) \\{x_{m\; i\; n} = {2.1459\sqrt{\lambda_{m\; i\; n}}}} & (435) \\{{ORIEN} = {\tan^{- 1}\left\lbrack \frac{P_{12}}{\lambda_{maj} - P_{11}} \right\rbrack}} & (436)\end{matrix}$10. Outputting to a display computer.

It will be understood that many additional changes in the details,materials, steps and arrangement of parts, which have been hereindescribed and illustrated in order to explain the nature of theinvention, may be made by those skilled in the art within the principleand scope of the invention as expressed in the appended claims.

1. A multi-stage target estimation method for underwater and airbornetargets, said method comprising the steps of: receiving data inputs froma target tracker; smoothing angle measurements at the endpoints of thedata inputs; searching a coarse grid in endpoint coordinates to producean initial target state estimate at two time lines, the time line 1 andthe time line 2; performing a Gauss-Newton maximum endpoint coordinatelikelihood estimate at the two time lines; calculating a Cartesiancoordinate maximum likelihood estimate subsequent to said step ofperforming a Gauss-Newton maximum endpoint coordinate likelihoodestimate; and outputting a target state to a display computer.
 2. Amulti-stage target estimation method as in claim 1 wherein said step ofcoarse grid searching comprises the steps of: selecting three frequencymeasurements from a target sensor and assigning, as a base frequency, amost recent frequency measurement; setting a minimum and maximum rangeat the time line 1 with respect to a sensor associated with the timeline 1; setting a minimum and maximum range at the time line 2 withrespect to a sensor associated with the time line 2; computing threevalues of range at time line 1 with respect to the sensor associatedwith the time line 1; computing three values of range at time line 2with respect to the sensor associated with the time line 2; processingfrequency data by computing five base frequency estimates; computing anendpoint coordinate performance index for each combination of thecomputed values of range and the computed base frequency estimates; andselecting the initial target state estimate of the computed values ofrange and the computed base frequency estimate associated with asmallest performance index.
 3. A multi-stage target estimation method asin claim 2 wherein said step of performing a Gauss-Newton endpointcoordinate maximum likelihood estimation comprises the steps of:initializing values for the range at the time line 1 with respect to thesensor associated with the time line 1 using the estimates from saidstep of course grid searching; initializing values for the range at thetime line 2 with respect to the sensor associated with the time line 2using the estimates from said step of course grid searching;initializing the base frequency state using the computed base frequencyestimate from said step of course grid searching; and performingGauss-Newton iterations until an endpoint coordinate maximum likelihoodis estimated.
 4. A multi-stage target estimation method as in claim 3wherein said step of performing Gauss-type iterations comprises thesteps of: computing endpoint parameters at the time of measurement ofthe time lines 1 and 2; computing a matrix of partial derivatives;performing a Householder transformation on the matrix; extracting anupper matrix from the Householder transformed matrix; setting a Y vectorto a last column of the Householder transformed matrix; computing a gainvector; and updating the states in which the states represent theendpoint coordinate maximum likelihood estimates at the two time lines.5. A multi-stage target estimation method as in claim 4 wherein saidstep of calculating a Cartesian coordinate maximum likelihood estimatecomprises the steps of: initializing values for an x-coordinate oftarget position from the Gauss-Newton endpoint coordinate maximumlikelihood estimate; initializing values for a y-coordinate of targetposition from the Gauss-Newton endpoint coordinate maximum likelihoodestimate; initializing values for an x-coordinate of target velocityfrom the Gauss-Newton endpoint coordinate maximum likelihood estimate;initializing values for a y-coordinate of target velocity from theGauss-Newton endpoint coordinate maximum likelihood estimate; andperforming Gauss-Newton iterations until the Cartesian coordinatemaximum likelihood is estimated.
 6. A multi-stage target estimationmethod as in claim 5 wherein said step of performing Gauss-typeiterations for Cartesian coordinate maximum likelihood comprises thesteps of: computing Cartesian endpoint parameters at the time ofmeasurement of the time lines 1 and 2; computing an assigned matrix ofpartial derivatives; performing a Householder transformation on theassigned matrix; extracting an upper matrix from the assignedHouseholder transformed matrix; setting an assigned Y vector to a lastcolumn of the assigned Householder transformed matrix; computing anassigned gain vector; and updating the states in which the statesrepresent the Cartesian coordinate maximum likelihood estimates at thetwo time lines at current time.
 7. A multi-stage target estimationmethod as in claim 6 wherein said step of outputting a target state to adisplay computer comprises the steps of: computing a Cartesiancoordinate covariance matrix; extrapolating the covariance matrix tocurrent time; computing target range; computing target bearing;computing target course; computing target speed; computing basefrequency standard deviations; computing major and minor localizationellipse axis length and orientation of major axis from North; andoutputting as the target state.
 8. A multi-stage target estimationmethod as in claim 7 wherein said step of coarse grid searching furthercomprises constraining a target to lie on azimuthal bearing lines.
 9. Amulti-stage target estimation method as in claim 7 wherein said step ofcoarse grid searching further comprises constraining a target to lie onconical angle hyperbolas.
 10. A multi-stage target estimation method asin claim 7 wherein said step of coarse grid searching further comprisesconstraining a target to lie on conical angle hyperbolic asymptotes.